#include "blockrelay/graphene_set.h"
#include "bloom.h"
#include "hash.h"
#include "serialize.h"
#include "streams.h"
#include "test/test_bitcoin.h"

#include <boost/test/unit_test.hpp>
#include <cassert>
#include <cmath>
#include <iostream>

// Create a deterministic hash by providing an index
uint256 GetHash(unsigned int nIndex)
{
    std::stringstream ss;
    ss << std::setfill('0') << std::setw(32) << nIndex;
    return uint256S(ss.str());
}

BOOST_FIXTURE_TEST_SUITE(graphene_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(graphene_set_encodes_and_decodes)
{
    uint256 senderArr[] = {
        SerializeHash(3), SerializeHash(1), SerializeHash(2), SerializeHash(7), SerializeHash(11), SerializeHash(4)};
    std::vector<uint256> senderItems(senderArr, senderArr + sizeof(senderArr) / sizeof(uint256));
    uint256 receiverArr[] = {
        SerializeHash(7), SerializeHash(2), SerializeHash(4), SerializeHash(-1), SerializeHash(1), SerializeHash(11)};
    std::vector<uint256> receiverItems(receiverArr, receiverArr + sizeof(receiverArr) / sizeof(uint256));

    // unordered graphene sets
    {
        CGrapheneSet senderGrapheneSet(6, senderItems, false, true);
        std::vector<uint64_t> reconciledCheapHashes = senderGrapheneSet.Reconcile(receiverItems);

        std::vector<uint64_t> senderCheapHashes;
        for (uint256 item : senderItems)
            senderCheapHashes.push_back(item.GetCheapHash());

        std::sort(senderCheapHashes.begin(), senderCheapHashes.end(), [](uint64_t i1, uint64_t i2) { return i1 < i2; });
        std::sort(reconciledCheapHashes.begin(), reconciledCheapHashes.end(),
            [](uint64_t i1, uint64_t i2) { return i1 < i2; });

        BOOST_CHECK_EQUAL_COLLECTIONS(reconciledCheapHashes.begin(), reconciledCheapHashes.end(),
            senderCheapHashes.begin(), senderCheapHashes.end());
    }

    // ordered graphene sets
    {
        CGrapheneSet senderGrapheneSet(6, senderItems, true, true);
        std::vector<uint64_t> reconciledCheapHashes = senderGrapheneSet.Reconcile(receiverItems);

        std::vector<uint64_t> senderCheapHashes;
        for (uint256 item : senderItems)
            senderCheapHashes.push_back(item.GetCheapHash());

        BOOST_CHECK_EQUAL_COLLECTIONS(reconciledCheapHashes.begin(), reconciledCheapHashes.end(),
            senderCheapHashes.begin(), senderCheapHashes.end());
    }
}

BOOST_AUTO_TEST_CASE(graphene_set_decodes_multiple_sizes)
{
    size_t nItemList[] = {1, 10, 50, 500, 5000, 10000};
    int nNumHashes = 0;
    for (size_t nItems : nItemList)
    {
        std::vector<uint256> senderItems;
        std::vector<uint64_t> senderCheapHashes;
        std::vector<uint256> baseReceiverItems;
        for (size_t i = 1; i <= nItems; i++)
        {
            nNumHashes++;
            const uint256 &hash = GetHash(nNumHashes);
            senderItems.push_back(hash);
            senderCheapHashes.push_back(hash.GetCheapHash());
            baseReceiverItems.push_back(hash);
        }

        // Add 10 more items to receiver mempool
        {
            std::vector<uint256> receiverItems = baseReceiverItems;
            for (size_t j = 1; j < 11; j++)
            {
                nNumHashes++;
                receiverItems.push_back(SerializeHash(GetHash(nNumHashes)));
            }

            CGrapheneSet senderGrapheneSet(receiverItems.size(), senderItems, true, true);
            std::vector<uint64_t> reconciledCheapHashes = senderGrapheneSet.Reconcile(receiverItems);

            BOOST_CHECK_EQUAL_COLLECTIONS(reconciledCheapHashes.begin(), reconciledCheapHashes.end(),
                senderCheapHashes.begin(), senderCheapHashes.end());
        }

        // Add 100 more items to receiver mempool
        {
            std::vector<uint256> receiverItems = baseReceiverItems;
            for (size_t j = 1; j < 101; j++)
            {
                nNumHashes++;
                receiverItems.push_back(SerializeHash(GetHash(nNumHashes)));
            }

            CGrapheneSet senderGrapheneSet(receiverItems.size(), senderItems, true, true);
            std::vector<uint64_t> reconciledCheapHashes = senderGrapheneSet.Reconcile(receiverItems);

            BOOST_CHECK_EQUAL_COLLECTIONS(reconciledCheapHashes.begin(), reconciledCheapHashes.end(),
                senderCheapHashes.begin(), senderCheapHashes.end());
        }

    }
}

BOOST_AUTO_TEST_CASE(graphene_set_finds_optimal_settings)
{
    const int SERIALIZATION_OVERHEAD = 11;
    FastRandomContext insecure_rand(true);
    CGrapheneSet grapheneSet;

    int m = 5000;
    int mu = 2999;
    int n = 3000;

    auto fpr = [m, mu](int a) { return a / float(m - mu); };

    int best_a = 1;
    size_t best_size = std::numeric_limits<size_t>::max();
    int a = 1;
    for (a = 1; a < m - mu; a++)
    {
        CBloomFilter filter(n, fpr(a), insecure_rand.rand32(), BLOOM_UPDATE_ALL, true, std::numeric_limits<uint32_t>::max());
        CIblt iblt(a);

        size_t filterBytes = ::GetSerializeSize(filter, SER_NETWORK, PROTOCOL_VERSION) - SERIALIZATION_OVERHEAD;
        size_t ibltBytes = ::GetSerializeSize(iblt, SER_NETWORK, PROTOCOL_VERSION) - SERIALIZATION_OVERHEAD;
        size_t total = filterBytes + ibltBytes;

        if (total < best_size)
        {
            best_size = total;
            best_a = a;
        }
    }

    BOOST_CHECK_EQUAL(grapheneSet.OptimalSymDiff(n, m), best_a);
}

BOOST_AUTO_TEST_CASE(graphene_set_can_serde)
{
    std::vector<uint256> senderItems;
    CDataStream ss(SER_DISK, 0);

    senderItems.push_back(SerializeHash(3));
    CGrapheneSet sentGrapheneSet(1, senderItems, true);
    CGrapheneSet receivedGrapheneSet;

    ss << sentGrapheneSet;
    ss >> receivedGrapheneSet;

    BOOST_CHECK_EQUAL(receivedGrapheneSet.Reconcile(senderItems)[0], senderItems[0].GetCheapHash());
}

BOOST_AUTO_TEST_CASE(item_rank_encodes_and_decodes)
{
    uint64_t itemArr[4] = {1, 20, 500, 7000};
    std::vector<uint64_t> inputItems(itemArr, itemArr + sizeof(itemArr) / sizeof(uint64_t));
    uint16_t nBits = 13;

    std::vector<unsigned char> encoded = CGrapheneSet::EncodeRank(inputItems, nBits);
    std::vector<uint64_t> outputItems = CGrapheneSet::DecodeRank(encoded, inputItems.size(), nBits);

    BOOST_CHECK_EQUAL_COLLECTIONS(outputItems.begin(), outputItems.end(), inputItems.begin(), inputItems.end());
}

BOOST_AUTO_TEST_SUITE_END()


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

#include "../coordsManip.hpp"

#include "../include/vts-browser/mapCallbacks.hpp" // ensure that projFinderCallback is visible

#include <boost/utility/in_place_factory.hpp>
#include <GeographicLib/Geodesic.hpp>
#include "geo/detail/projapi.hpp"

#include <vts-libs/vts/csconvertor.hpp>
#include <vts-libs/vts/mapconfig.hpp>

#include <unordered_map>
#include <memory>
#include <functional>

namespace vts
{

std::function<const char *(const char *)> &projFinderCallback()
{
    static std::function<const char *(const char *)> fnc;
    return fnc;
}

namespace
{

const char *pjFind(const char *p)
{
    if (!p)
        return nullptr; // nothing to look for? nothing to return.
    if (projFinderCallback())
        return projFinderCallback()(p);
    return nullptr;
}

// disable PROJ's file access on some systems
PAFile pjFOpen(projCtx ctx, const char *filename, const char *access)
{
    return nullptr;
}
size_t pjFRead(void *buffer, size_t size, size_t nmemb, PAFile file)
{
    return 0;
}
int pjFSeek(PAFile file, long offset, int whence)
{
    return 0;
}
long pjFTell(PAFile file)
{
    return 0;
}
void pjFClose(PAFile file)
{}

struct projInitClass
{
    projFileAPI_t pjFileApi;

    projInitClass()
    {
        pjFileApi.FOpen = &pjFOpen;
        pjFileApi.FRead = &pjFRead;
        pjFileApi.FSeek = &pjFSeek;
        pjFileApi.FTell = &pjFTell;
        pjFileApi.FClose = &pjFClose;
        pj_set_finder(&pjFind);
    }
} projInitInstance;

class CoordManipImpl : public CoordManip
{
public:
    vtslibs::vts::MapConfig &mapconfig;
    std::unordered_map<std::string, std::unique_ptr<vtslibs::vts::CsConvertor>> convertors;
    boost::optional<GeographicLib::Geodesic> geodesic_;
    projCtx ctx = nullptr;

    CoordManipImpl(
            vtslibs::vts::MapConfig &mapconfig,
            const std::string &searchSrs,
            const std::string &customSrs1,
            const std::string &customSrs2) :
        mapconfig(mapconfig),
        ctx(pj_ctx_alloc())
    {
        LOG(info1) << "Creating coordinate systems manipulator";

#ifdef __EMSCRIPTEN__
        pj_ctx_set_fileapi(ctx, &projInitInstance.pjFileApi);
#endif

        // create geodesic
        {
            auto r = geo::ellipsoid(mapconfig.srs(mapconfig.referenceFrame.model.navigationSrs).srsDef);
            auto a = r[0];
            auto b = r[2];
            LOG(info1) << "Creating geodesic manipulator: a=<" << a << ">, b=<" << b << ">";
            geodesic_ = boost::in_place(a, (a - b) / a);
        }

        addSrsDef("$search$", searchSrs);
        addSrsDef("$custom1$", customSrs1);
        addSrsDef("$custom2$", customSrs2);
    }

    ~CoordManipImpl()
    {
        pj_ctx_free(ctx);
    }

    void addSrsDef(const std::string &name, const std::string &def)
    {
        vtslibs::registry::Srs s;
        s.comment = name;
        s.srsDef = geo::SrsDefinition::fromString(def);
        mapconfig.srs.replace(name, s);
    }

    const std::string &srsToProj(Srs srs)
    {
        static const std::string search = "$search$";
        static const std::string custom1 = "$custom1$";
        static const std::string custom2 = "$custom2$";
        switch(srs)
        {
        case Srs::Physical:
            return mapconfig.referenceFrame.model.physicalSrs;
        case Srs::Navigation:
            return mapconfig.referenceFrame.model.navigationSrs;
        case Srs::Public:
            return mapconfig.referenceFrame.model.publicSrs;
        case Srs::Search:
            return search;
        case Srs::Custom1:
            return custom1;
        case Srs::Custom2:
            return custom2;
        default:
            LOGTHROW(fatal, std::invalid_argument) << "Invalid srs enum";
            throw;
        }
    }

    vtslibs::vts::CsConvertor &convertor(const std::string &a, const std::string &b)
    {
        const std::string key = a + " >>> " + b;
        auto it = convertors.find(key);
        if (it == convertors.end())
        {
            convertors[key] = std::make_unique<vtslibs::vts::CsConvertor>(a, b, mapconfig, ctx);
            it = convertors.find(key);
        }
        return *it->second;
    }

    vec3 convert(const vec3 &value, const std::string &f, const std::string &t)
    {
        const auto &cs = convertor(f, t);
        vec3 res = vecFromUblas<vec3>(cs(vecFromUblas<math::Point3>(value)));
        //LOG(debug) << "Converted <" << value.transpose() << "><" << f << "> to <" << res.transpose() << "><" << t << ">";
        return res;
    }
};

} // namespace

std::shared_ptr<CoordManip> CoordManip::create(
    vtslibs::vts::MapConfig &mapconfig,
    const std::string &searchSrs,
    const std::string &customSrs1,
    const std::string &customSrs2)
{
    return std::make_shared<CoordManipImpl>(mapconfig, searchSrs, customSrs1, customSrs2);
}

vec3 CoordManip::navToPhys(const vec3 &value)
{
    return convert(value, Srs::Navigation, Srs::Physical);
}

vec3 CoordManip::physToNav(const vec3 &value)
{
    return convert(value, Srs::Physical, Srs::Navigation);
}

vec3 CoordManip::searchToNav(const vec3 &value)
{
    return convert(value, Srs::Search, Srs::Navigation);
}

vec3 CoordManip::convert(const vec3 &value, Srs from, Srs to)
{
    CoordManipImpl *impl = (CoordManipImpl *)this;
    return impl->convert(value, impl->srsToProj(from), impl->srsToProj(to));
}

vec3 CoordManip::convert(const vec3 &value, const std::string &from, Srs to)
{
    CoordManipImpl *impl = (CoordManipImpl *)this;
    return impl->convert(value, from, impl->srsToProj(to));
}

vec3 CoordManip::convert(const vec3 &value, Srs from, const std::string &to)
{
    CoordManipImpl *impl = (CoordManipImpl *)this;
    return impl->convert(value, impl->srsToProj(from), to);
}

vec3 CoordManip::geoDirect(const vec3 &position, double distance, double azimuthIn, double &azimuthOut)
{
    CoordManipImpl *impl = (CoordManipImpl *)this;
    vec3 res;
    impl->geodesic_->Direct(position(1), position(0), azimuthIn, distance, res(1), res(0), azimuthOut);
    res(2) = position(2);
    return res;
}

vec3 CoordManip::geoDirect(const vec3 &position, double distance, double azimuthIn)
{
    double a;
    return geoDirect(position, distance, azimuthIn, a);
}

void CoordManip::geoInverse(const vec3 &a, const vec3 &b, double &distance, double &azimuthA, double &azimuthB)
{
    CoordManipImpl *impl = (CoordManipImpl *)this;
    impl->geodesic_->Inverse(a(1), a(0), b(1), b(0), distance, azimuthA, azimuthB);
}

double CoordManip::geoAzimuth(const vec3 &a, const vec3 &b)
{
    double d, a1, a2;
    geoInverse(a, b, d, a1, a2);
    return a1;
}

double CoordManip::geoDistance(const vec3 &a, const vec3 &b)
{
    double d, a1, a2;
    geoInverse(a, b, d, a1, a2);
    return d;
}

double CoordManip::geoArcDist(const vec3 &a, const vec3 &b)
{
    CoordManipImpl *impl = (CoordManipImpl *)this;
    double dummy;
    return impl->geodesic_->Inverse(a(1), a(0), b(1), b(0), dummy);
}

} // namespace vts


//  bpp.cpp
//  PhyloAcc
//
//  Created by hzr on 3/8/16.
//  Copyright © 2016 hzr. All rights reserved.
//

#include "bpp.hpp"
#include <armadillo>
#include <sys/types.h>
#include <dirent.h>
#include<queue>

#include <cmath>
#include <cassert>

#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_cdf.h>

#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <ctype.h>

#include "newick.h"
#include "utils.h"
#include "bpp_c.hpp"

using namespace std;
using namespace arma;

// load the phylogenetic tree
void BPP::InitPhyloTree(PhyloTree & tree) //, double indel_pi), double indel, double indel2
{

    submat = tree.subs_rate;
    children    = new int[N][2];
    parent      = new int[N];
    distances   = new double[N];
    thetas   = new double[N];
    heights = vector<double>(N);
    move_br = vector<int>(N);

    for(int s=0; s<N; s++)
    {
        distances[s] = tree.distances[s];
        thetas[s] = tree.thetas[s];
        heights[s] = 0; // root
    }

    for(int i=0; i<N; i++)
    {
        children[i][0] = -1;
        children[i][1] = -1;
        parent[i] = N;
    }

    for(int i=0; i<N; i++)
    {
        int p = -1;
        for(int j=0; j<N; j++)
        {
            if (tree.dag[i][j])
            {
                p++;
                children[i][p] = j;
                parent[j] = i;
            }
        }
    }

    for(int i = N-2; i >= 0; i--)
    {
        heights[i] = heights[parent[i]] - distances[i]; //heights<=0. root=0. Distance: br length. +ve.
    }
}

// try to match the phylogenetic profile and tree
void BPP::MatchProfAndTree(PhyloProf & _prof, PhyloTree & _tree)
{
    // try to match the species
    bool success_match = true;
    int S = _tree.S;
    int S2 = _prof.S;
    vector<int> reorder(S);  //each species in the tree where is in prof
    for(int s1=0; s1<S; s1++)
    {
        bool has_same_species = false;
        string sname1 = _tree.species_names[s1];
        for(int s2=0; s2<S2; s2++)
        {
            string sname2 = _prof.species_names[s2];
            if (sname1 == sname2)
            {
                has_same_species = true;
                reorder[s1] = s2;
                break;
            }
        }
        if (!has_same_species)
        {
            cout << "No matrix species " << _prof.species_names[s1] << " found in tree." << endl;
            success_match = false;
            break;
        }
    }

    if (!success_match) // if cannot match literally
    {
        cout << endl << "The species in phylogenetic profile and tree cannot be matched literally:" << endl;
        cout << "The program will use the default mapping in data:" << endl;
        for(int s=0; s<S; s++)
            cout << "(" << _prof.species_names[s] << "\t=  " << _tree.species_names[s] << ")" << endl;
        cout << endl;
    }
    else                // if successully matched
    {
        cout << "The species in profile and tree match perfectly. Reorder the species in profile matrix by the tree." << endl << endl;
        vector<string> old_X = _prof.X;
        for(int s=0; s<S; s++)
        {
            int reorder_s = reorder[s];
            _prof.X[s] = old_X[reorder_s];
        }
        _prof.species_names = _tree.species_names;
    }

}

void BPP::InitMCMC(int _num_burn, int _num_mcmc, int _num_thin)
{
    // init parameters
    num_burn = _num_burn;

    num_mcmc = _num_mcmc;
    num_thin = _num_thin;

    last_time = time(NULL);

    // init MCMC sampling storage
    Max_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));
    cur_Z = vector<vector < vector <int > >> (3,vector < vector <int >>  (C,vector <int > (N,0)));

    genetrees = vector<vector<string>>(3, vector<string>(C)); // for max gene tree

    log_liks_null = vector <double>(C,0);
    log_liks_null_L = vector <double>(C,0);
    log_liks_Z = vector<vector <double>>(3,vector <double>(C,0));
    log_liks_sgl = vector <double>(C,0);
    log_liks_resZ = vector <double>(C,0);
    log_liks_sgl_L = vector <double>(C,0);
    log_liks_resZ_L = vector <double>(C,0);
    log_liks_WL = vector< vector <double> > (3, vector <double>(C, 0));
    log_mle = vector<vector<double> > (3, vector<double> (C,0));

    log_liks_curZ = vector <double>(C,0);
    log_liks_propZ = vector <double>(C,0);
    MH_ratio_gain = vector <double>(C,0);
    MH_ratio_loss = vector <double>(C,0);

    cur_crate = vector <double>(C,ratio0);
    cur_nrate = vector <double>(C,ratio1);

    cur_lrate = vector <double>(C,ind_lrate);
    cur_lrate2 = vector <double>(C,ind_lrate2);
    cur_grate = vector <double>(C, ind_grate);

   //Han*: used to save mode of MCMC output, so one element one vector.
    cur_pi = vector<vector<vector<double>>>(3,vector<vector<double>> (C, vector<double>(4,0.25)));

}

void BPP::sample_proposal(int iter, double & lrate_prop, double & grate_prop, ofstream & output)
{
    lrate_prop =gsl_ran_beta(RNG, ind_lrate * vlr, (1 - ind_lrate) *vlr); // let vlr == vgr!!
    grate_prop =gsl_ran_beta(RNG, ind_grate * vgr, (1 - ind_grate - ind_lrate) *vgr); //gsl_ran_gamma(RNG, vgr, ind_grate/vgr);
    grate_prop = grate_prop * (1 - lrate_prop);

    output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< indel << "\t"<< indel2 << "\t"<<ind_grate<< "\t"<< ind_lrate <<endl;
}

vec BPP::getlogTM(double dist, double rate, mat & lam )
{
    mat tmp_diag  = exp(eigenval* dist * rate);
    mat x = eigenvec;
    x.each_col()%=tmp_diag;

    mat log_cache_TM = log(eigeninv * x) ; //transpose Q
    return(log_multi(log_cache_TM, lam.col(lam.n_cols -1)));
}

mat BPP::getlogTM(double dist, double rate) //Han: get log transition matrix.
{
    if(dist < 1e-8)
    {
        mat x(num_base, num_base);
        x.fill(-INFINITY);
        x.diag().zeros();
        return(x);
    }
    mat tmp_diag  = exp(eigenval* dist * rate);
    mat x = eigenvec;
    x.each_col()%=tmp_diag;

    return(log(eigeninv * x)) ; //transpose Q

}

//Han*: new function. to account for updated Q
//For each element, since eigen value/vec changed
mat BPP::getlogTMc(double dist, double rate, mat& c_eigenvec, mat& c_eigenval, mat& c_eigeninv)
{
    if(dist < 1e-8)
    {
        mat x(num_base, num_base);
        x.fill(-INFINITY);
        x.diag().zeros();
        return(x);
    }
    mat tmp_diag  = exp(c_eigenval* dist * rate);
    mat x = c_eigenvec;
    x.each_col()%=tmp_diag;

    return(log(c_eigeninv * x)) ; //transpose Q //element-wise log
}

mat BPP::getlogTM_len(double dist, mat& c_eigenvec, mat& c_eigenval, mat& c_eigeninv){
    if(dist < 1e-8){
        mat x(num_base, num_base);
        x.fill(-INFINITY);
        x.diag().zeros();
        return(x);
    }
    mat tmp_diag  = exp(c_eigenval* dist);
    mat x = c_eigenvec;
    x.each_col()%=tmp_diag;

    return(log(c_eigeninv * x)) ; //transpose Q
}

double BPP::log_lik(vector< vector<vec> > & lambda, double _indel, double _indel2, int start1, int end1, vector<unsigned int> & v, double p)
{
    // compute loglik
    double result =0;
    mat x(2,2);

    int rr = *subtree.rbegin();
    // 1. sending the lambda msg from leaves bottom up through the network
    for(vector<int>::iterator it = subtree.begin(); it!=subtree.end(); it++) //int s=S; s<N; s++)
    {
        int s = *it;
        if(s<S) continue;
        int* p = children[s];
        for(int it = start1; it < end1; it++)  lambda[v[it]][s].fill(0);

        for(int cc=0;cc<2;cc++)
        {
            int chi = p[cc];
            assert(chi != -1);
            if(distances[chi]>0 )
            {
                double tt = (1 - exp(-(_indel + _indel2) * distances[chi]))/(_indel + _indel2);
                x.at(1,0) = _indel * tt;
                x.at(0,0) = 1 - x.at(1,0);

                x.at(0,1) = _indel2 * tt;
                x.at(1,1) = 1 - x.at(0,1);

                //cout << x;
                x = log(x);

            }
            else{
                x.fill(-INFINITY); //83, root
                x.diag().fill(0);
            }

            #pragma omp parallel for schedule (guided)
            for(int it = start1; it < end1; it++)  lambda[v[it]][s] +=  BPP::log_multi(x,lambda[v[it]][chi]);
        }

    }

    // 2. processing the distribution of root species
    for(int it = start1; it < end1; it++)
    {

        lambda[v[it]][rr][0] += log(1-p); //N-1
        lambda[v[it]][rr][1] += log(p) ;
        result += BPP::log_exp_sum(lambda[v[it]][rr]);
    }

    return(result);
}

void BPP::sample_hyperparam(int iter, vector<int> & ids, ofstream & output) // recompute log_TM, double indel_prop, double indel2_prop,
{
    //indepent MH to sample hyperparam of rates
    double p=1,r = 1; //hyperparam for shape
    double q=0.1,s = 0.1; // hyperparam for scale

    double vna = 100, vnb = 100, vca = 100, vcb = 100;
    double nprior_a_prop =gsl_ran_gamma(RNG, vna, nprior_a/vna);
    double cprior_a_prop =gsl_ran_gamma(RNG, vca, cprior_a/vca);

    double nprior_b_prop =gsl_ran_gamma(RNG, vnb, nprior_b/vnb);
    double cprior_b_prop =gsl_ran_gamma(RNG, vcb, cprior_b/vcb);

    //MH proposal
    double sum_r = 0;
    double log_prod_r = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        sum_r += cur_nrate[c];
        //var_r += pow(*it, 2);
        log_prod_r += log(cur_nrate[c]);
    }

    double M_ratio = (nprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b_prop - (ids.size() + r)*lgamma(nprior_a_prop) - log(nprior_b_prop) * nprior_a_prop * (s + ids.size());
    M_ratio -= (nprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/nprior_b - (ids.size() + r)*lgamma(nprior_a) - log(nprior_b) * nprior_a * (s + ids.size());

    double H_ratio = log(gsl_ran_gamma_pdf(nprior_a,vna,nprior_a_prop/vna)) - log(gsl_ran_gamma_pdf(nprior_a_prop,vna,nprior_a/vna)) + log(gsl_ran_gamma_pdf(nprior_b,vnb,nprior_b_prop/vnb)) - log(gsl_ran_gamma_pdf(nprior_b_prop,vnb,nprior_b/vnb));

    cout << "nrate_MH_ratio: " << M_ratio <<", " << H_ratio << ", " << nprior_a << ", " << nprior_a_prop << ", " << nprior_b << ", " << nprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        nprior_a = nprior_a_prop;
        nprior_b = nprior_b_prop;
    }

    sum_r = 0;
    log_prod_r = 0;
    //for(vector<double>::iterator it = cur_crate.begin(); it< cur_crate.end(); it++)
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        sum_r += cur_crate[c];
        //var_r += pow(*it, 2);
        log_prod_r += log(cur_crate[c]);
    }

    M_ratio = (cprior_a_prop - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b_prop - (ids.size() + r)*lgamma(cprior_a_prop) - log(cprior_b_prop) * cprior_a_prop * (s + ids.size()) - ((cprior_a - 1) * (log(p) + log_prod_r) - (q + sum_r)/cprior_b - (ids.size() + r)*lgamma(cprior_a) - log(cprior_b) * cprior_a * (s + ids.size()));

    H_ratio = log(gsl_ran_gamma_pdf(cprior_a,vca,cprior_a_prop/vca)) - log(gsl_ran_gamma_pdf(cprior_a_prop,vca,cprior_a/vca)) + log(gsl_ran_gamma_pdf(cprior_b,vcb,cprior_b_prop/vcb)) - log(gsl_ran_gamma_pdf(cprior_b_prop,vcb,cprior_b/vcb));

    cout << "crate_MH_ratio: " << M_ratio + H_ratio << ", " << cprior_a << ", " << cprior_a_prop <<", " << cprior_b << ", " << cprior_b_prop << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        cprior_a = cprior_a_prop;
        cprior_b = cprior_b_prop;
    }

  // sample hyperparameters of lrate and grate, exponential prior for prior_l_a and prior_l_b, prior_g_a and prior_g_b
    double u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_l_a_prop = prior_l_a *  u;

    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_l_b_prop = prior_l_b *  u;
    double log_p = 0, log_pc = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        log_p += log(cur_lrate[c]);
        log_pc += log(1 - cur_lrate[c]);
    }

    M_ratio = (prior_l_a_prop - prior_l_a) * (log_p - 1) + (prior_l_b_prop - prior_l_b) * (log_pc - 1);
    M_ratio += ids.size() * (gsl_sf_lnbeta(prior_l_a, prior_l_b) - gsl_sf_lnbeta(prior_l_a_prop, prior_l_b_prop));

    H_ratio = log(prior_l_a) - log(prior_l_a_prop) + log(prior_l_b) - log(prior_l_b_prop);

    cout << "lrate_MH_ratio: " << M_ratio + H_ratio << ", " << prior_l_a << ", " << prior_l_a_prop <<", " << prior_l_b << ", " << prior_l_b_prop  << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        prior_l_a = prior_l_a_prop;
        prior_l_b = prior_l_b_prop;
    }

    // sample grate
    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_g_a_prop = prior_g_a *  u;

    u = gsl_rng_uniform(RNG)*(1.3 - 0.7) + 0.7; // generate uniform from (1/prop_n, prop_n);
    double prior_g_b_prop = prior_g_b *  u;
    log_p = 0; log_pc = 0;
    for(std::size_t i = 0; i < ids.size(); i++ )  // 0702
    {
        int c = ids[i];
        log_p += log(cur_grate[c]);
        log_pc += log(1 - cur_grate[c]);
    }

    M_ratio = (prior_g_a_prop - prior_g_a) * (log_p - 1) + (prior_g_b_prop - prior_g_b) * (log_pc - 1);
    M_ratio += ids.size() * (gsl_sf_lnbeta(prior_g_a, prior_g_b) - gsl_sf_lnbeta(prior_g_a_prop, prior_g_b_prop));

    H_ratio = log(prior_g_a) - log(prior_g_a_prop) + log(prior_g_b) - log(prior_g_b_prop);

    cout << "grate_MH_ratio: " << M_ratio + H_ratio << ", " << prior_g_a << ", " << prior_g_a_prop <<", " << prior_g_b << ", " << prior_g_b_prop  << endl;

    if(log(gsl_rng_uniform(RNG)) < M_ratio + H_ratio)
    {
        prior_g_a = prior_g_a_prop;
        prior_g_b = prior_g_b_prop;
    }

    output << iter << "\t"<< nprior_a<< "\t"<< nprior_b <<"\t"<< cprior_a << "\t"<< cprior_b << "\t"<< prior_l_a << "\t"<< prior_l_b << "\t"<< prior_g_a << "\t"<< prior_g_b <<endl;
}

void BPP::getUppertree(int root, vector<int>& child, set<int> & visited_init)  // go to the root, include root!
{
    for(vector<int>::iterator it = child.begin(); it!=child.end(); it++)
    {
       int p = *it;
       while(p!=root)
       {
           visited_init.insert(p);
           p = parent[p];
       }
    }
    visited_init.insert(root);
}

void BPP::getSubtree(int root, vector<int> & visited_init)  // traverse from root to children, include root
{

    int j = root;

    //cout << nodes_names[j]<<"\t";

    if(children[j][0]!=-1)
    {
        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], visited_init);
        }
    }
    visited_init.push_back(j);
}

void BPP::getSubtree(int root, set<int>& child, vector<int> & visited_init)  // traverse from root, stop at children, 74 & 64; do include 1-S!
{
    int j = root;

    //cout << nodes_names[j]<<"\t";
    if(child.find(j) != child.end())
    {
        visited_init.push_back(j);
        return;
    }

    if(children[j][0]!=-1)
    {
        for(int chi=0;chi<2;chi++)
        {
            getSubtree(children[j][chi], child, visited_init);
        }
    }
     visited_init.push_back(j);
}

void BPP::Output_init(PhyloProf & prof, string output_path, vector<int> & ids){

        string outpath_elem = output_path+ "_elem_lik.txt";
        ofstream out_lik(outpath_elem.c_str());
        out_lik.precision(8);

        //out_lik<<"No.\tID\tloglik_Null_W\tloglik_Acc_W\tloglik_Full_W\tlogBF1\tlogBF2\tlogPost_Max_M0\tlogPost_Max_M1\tlogPost_Max_M2\n";
        out_lik<<"No.\tID\tloglik_Null_W\tloglik_Acc_W\tloglik_Full_W\tlogBF1\tlogBF2"<<endl;
        for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
        {
            int cc = *it;
            out_lik <<cc << "\t" << prof.element_names[cc] << "\t" << log_liks_WL[0][cc] <<"\t"<< log_liks_WL[2][cc] <<"\t"<< log_liks_WL[1][cc] <<"\t";
            out_lik<<log_liks_WL[2][cc]-log_liks_WL[0][cc]<<"\t"<<log_liks_WL[2][cc]-log_liks_WL[1][cc]<<endl;
            //out_lik <<log_liks_Z[0][cc] << "\t" <<log_liks_Z[2][cc]<<"\t" <<log_liks_Z[1][cc]<<"\t";
            //out_lik<<log_mle[0][cc]<<"\t"<<log_mle[2][cc]<<"\t"<<log_mle[1][cc]<<"\t"<<log_mle[2][cc]-log_mle[0][cc]<<"\t"<<log_mle[2][cc]-log_mle[1][cc];
            //out_lik << endl;
        }

        out_lik.close();

    ofstream out_z;
    for(int r =0;r<3;r++)
    {
      if(r == 2)
      {
          outpath_elem = output_path+"_M" +to_string(1) + "_elem_Z.txt";
      }else if(r == 1)
      {
          outpath_elem = output_path+"_M" +to_string(2) + "_elem_Z.txt";
      }else{
          outpath_elem = output_path+"_M" +to_string(0) + "_elem_Z.txt";
      }

            out_z.open(outpath_elem.c_str());
            out_z<<"No.";
                for(int s =0 ;s<N;s++){  // header: species name
                    out_z<< "\t" << nodes_names[s];
                }
            out_z << "\tgenetree" <<endl;

            for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
            {
                int c = *it;
                out_z<<c;
                for(int s=0; s<N;s++)
                    out_z<<"\t" << Max_Z[r][c][s];
                out_z << "\t" << genetrees[r][c];
                out_z <<endl;
            }
        out_z.close();
    }

    ofstream out_pi;
    for(int r=0; r<3;r++){
        if(r==0){
            outpath_elem=output_path+"_M"+to_string(0)+"_Beta_Post_pi_mode.txt";
        }else if(r==2){
            outpath_elem=output_path+"_M"+to_string(1)+"_Beta_Post_pi_mode.txt";
        }else{
            outpath_elem=output_path+"_M"+to_string(2)+"_Beta_Post_pi_mode.txt";
        }
        out_pi.open(outpath_elem.c_str());

        for(vector<int>:: iterator it = ids.begin(); it !=ids.end(); it++)
        {
            int c=*it;
            out_pi << c;
            for(int b=0; b<4; b++) {//4 or num_base
                out_pi<<"\t"<<cur_pi[r][c][b];
            }
            out_pi <<endl;
        }
        out_pi.close();
    }

}

void BPP::Output_init0(PhyloProf & prof, ofstream& out_lik, vector<int> & ids){

    //for(int cc=0; cc<C;cc++)
    for(vector<int>::iterator it = ids.begin(); it !=ids.end(); it++)
    {
        int cc = *it;
        out_lik <<cc << "\t" << prof.element_names[cc] << "\t"  <<log_liks_sgl[cc]<< "\t"<<log_liks_Z[1][cc];
        out_lik << endl;
    }
}

/*** output simulated sequence ***/
void BPP::Output_simu(PhyloProf & prof, string outpath, int c){
    ofstream output;
    string outpath1 = outpath + ".fasta";
    string outpath2 = outpath + ".bed";
    string outpath3 = outpath +"_Z.txt";
    string outpath4 = outpath +"_pi.txt";
    output.open(outpath1.c_str());
    for(int s = 0; s< S; s++)
    {
        output << ">" << prof.species_names[s] << endl;
        output << prof.X[s].substr(0, element_start[c] + element_size[c])<< endl; //strutils::ToUpperCase(
    }
    output.close();

    output.open(outpath2.c_str());
    for(int i = 0; i < c; i++)
    {
        output << i << "\t" << element_start[i] << "\t" << element_start[i] + element_size[i]<< "\t";
        //Han*: add in output for pi
        output << i << "\t" << 1 << "\t" << cur_crate[i] << "\t" << cur_nrate[i] << "\t" << genetrees[0][i] << endl;
    }
    output.close();

    //Han*: output for Z
    output.open(outpath3.c_str());
    for(int s=0; s<N; s++){
        output<<"\t"<<nodes_names[s];
    }
    output<<endl;
    for(int s=0; s<N; s++){
        output<<"\t"<<cur_Z[0][0][s];
    }
    output<<endl;
    output.close();

    //Han*: output pi
    output.open(outpath4.c_str());
    for(int i=0; i<c; i++){
        output<<i<<"\t"<<cur_pi[0][i][0]<<endl; //only need to output pi_A (double-stranded)
    }
    output.close();
}

    void BPP::getTreeString(int rootN, std::stringstream & buffer)
    {
        if (children[rootN][0] == -1)
        {
            buffer << species_names[rootN] << ":"<< distances[rootN];
        }
        else
        {
            buffer << "(";
            for(int i =0;i <2; i++)
            {
                int child = children[rootN][i];

                getTreeString(child, buffer);
                if(i==0) buffer << ",";
            }

            if(parent[rootN] < N)
            {
                buffer << "):" << distances[rootN];
            }else{
                buffer << ");" ;
            }
        }
    }


/*
 * RecoTauDiscriminantCutMultiplexer
 *
 * Author: Evan K. Friis, UW
 *
 * Takes two PFTauDiscriminators.
 *
 * The "key" discriminantor is rounded to the nearest integer.
 *
 * A set of cuts for different keys on the "toMultiplex" discriminantor is
 * provided in the config file.
 *
 * Both the key and toMultiplex discriminators should map to the same PFTau
 * collection.
 *
 */
#include <boost/foreach.hpp>
#include "RecoTauTag/RecoTau/interface/TauDiscriminationProducerBase.h"
#include "FWCore/Utilities/interface/InputTag.h"
#include "CommonTools/Utils/interface/StringObjectFunction.h"
#include "DataFormats/TauReco/interface/PFTau.h"
#include "FWCore/ParameterSet/interface/FileInPath.h"

#include <FWCore/ParameterSet/interface/ConfigurationDescriptions.h>
#include <FWCore/ParameterSet/interface/ParameterSetDescription.h>

#include "CondFormats/PhysicsToolsObjects/interface/PhysicsTGraphPayload.h"
#include "CondFormats/DataRecord/interface/PhysicsTGraphPayloadRcd.h"
#include "CondFormats/PhysicsToolsObjects/interface/PhysicsTFormulaPayload.h"
#include "CondFormats/DataRecord/interface/PhysicsTFormulaPayloadRcd.h"

#include "TMath.h"
#include "TGraph.h"
#include "TFormula.h"
#include "TFile.h"

class RecoTauDiscriminantCutMultiplexer : public PFTauDiscriminationProducerBase
{
 public:
  explicit RecoTauDiscriminantCutMultiplexer(const edm::ParameterSet& pset);

  ~RecoTauDiscriminantCutMultiplexer() override;
  double discriminate(const reco::PFTauRef&) const override;
  void beginEvent(const edm::Event& event, const edm::EventSetup& eventSetup) override;

  static void fillDescriptions(edm::ConfigurationDescriptions & descriptions);

 private:
  std::string moduleLabel_;

  bool loadMVAfromDB_;
  edm::FileInPath inputFileName_;

  struct DiscriminantCutEntry
  {
    DiscriminantCutEntry()
      : cutVariable_(),
	cutFunction_(),
	mode_(kUndefined)
    {}
    ~DiscriminantCutEntry()
    {
    }
    double cutValue_;
    std::string cutName_;
    std::unique_ptr<StringObjectFunction<reco::PFTau>> cutVariable_;
    std::unique_ptr<const TGraph> cutFunction_;
    enum { kUndefined, kFixedCut, kVariableCut };
    int mode_;
  };
  typedef std::map<int, std::unique_ptr<DiscriminantCutEntry>> DiscriminantCutMap;
  DiscriminantCutMap cuts_;

  std::string mvaOutputNormalizationName_;
  std::unique_ptr<const TFormula> mvaOutput_normalization_;

  bool isInitialized_;

  edm::InputTag toMultiplex_;
  edm::InputTag key_;
  edm::Handle<reco::PFTauDiscriminator> toMultiplexHandle_;
  edm::Handle<reco::PFTauDiscriminator> keyHandle_;
  edm::EDGetTokenT<reco::PFTauDiscriminator> toMultiplex_token;
  edm::EDGetTokenT<reco::PFTauDiscriminator> key_token;

  int verbosity_;
};

namespace
{
  std::unique_ptr<TFile> openInputFile(const edm::FileInPath& inputFileName) {
    if ( inputFileName.location() == edm::FileInPath::Unknown){  throw cms::Exception("RecoTauDiscriminantCutMultiplexer::loadObjectFromFile")
      << " Failed to find File = " << inputFileName << " !!\n";
    }
    return std::unique_ptr<TFile>{ new TFile(inputFileName.fullPath().data()) };
  }

  template <typename T>
  std::unique_ptr<const T> loadObjectFromFile(TFile& inputFile, const std::string& objectName)
  {
    const T* object = dynamic_cast<T*>(inputFile.Get(objectName.data()));
    if ( !object )
      throw cms::Exception("RecoTauDiscriminantCutMultiplexer::loadObjectFromFile")
        << " Failed to load Object = " << objectName.data() << " from file = " << inputFile.GetName() << " !!\n";
    //Need to use TObject::Clone since the type T might be a base class
    return std::unique_ptr<const T>{ static_cast<T*>(object->Clone()) };
  }

  std::unique_ptr<const TGraph> loadTGraphFromDB(const edm::EventSetup& es, const std::string& graphName, const int& verbosity_ = 0)
  {
    if(verbosity_){
      std::cout << "<loadTGraphFromDB>:" << std::endl;
      std::cout << " graphName = " << graphName << std::endl;
    }
    edm::ESHandle<PhysicsTGraphPayload> graphPayload;
    es.get<PhysicsTGraphPayloadRcd>().get(graphName, graphPayload);
    return std::unique_ptr<const TGraph>{ new TGraph(*graphPayload.product()) };
  }

  std::unique_ptr<TFormula> loadTFormulaFromDB(const edm::EventSetup& es, const std::string& formulaName, const TString& newName, const int& verbosity_ = 0)
  {
    if(verbosity_){
      std::cout << "<loadTFormulaFromDB>:" << std::endl;
      std::cout << " formulaName = " << formulaName << std::endl;
    }
    edm::ESHandle<PhysicsTFormulaPayload> formulaPayload;
    es.get<PhysicsTFormulaPayloadRcd>().get(formulaName, formulaPayload);

    if ( formulaPayload->formulas().size() == 1 && formulaPayload->limits().size() == 1 ) {
      return std::unique_ptr<TFormula> {new TFormula(newName, formulaPayload->formulas().at(0).data()) };
    } else {
      throw cms::Exception("RecoTauDiscriminantCutMultiplexer::loadTFormulaFromDB")
	<< "Failed to load TFormula = " << formulaName << " from Database !!\n";
    }
    return std::unique_ptr<TFormula>{};
  }
}

RecoTauDiscriminantCutMultiplexer::RecoTauDiscriminantCutMultiplexer(const edm::ParameterSet& cfg)
  : PFTauDiscriminationProducerBase(cfg),
    moduleLabel_(cfg.getParameter<std::string>("@module_label")),
    mvaOutput_normalization_(),
    isInitialized_(false)
{

  toMultiplex_ = cfg.getParameter<edm::InputTag>("toMultiplex");
  toMultiplex_token = consumes<reco::PFTauDiscriminator>(toMultiplex_);
  key_ = cfg.getParameter<edm::InputTag>("key");
  key_token = consumes<reco::PFTauDiscriminator>(key_);

  verbosity_ = cfg.getParameter<int>("verbosity");

  loadMVAfromDB_ = cfg.getParameter<bool>("loadMVAfromDB");
  if ( !loadMVAfromDB_ ) {
      inputFileName_ = cfg.getParameter<edm::FileInPath>("inputFileName");
  }
  if(verbosity_)  std::cout << moduleLabel_ << " loadMVA = " << loadMVAfromDB_ << std::endl;
  mvaOutputNormalizationName_ = cfg.getParameter<std::string>("mvaOutput_normalization");

  // Setup our cut map
  typedef std::vector<edm::ParameterSet> VPSet;
  VPSet mapping = cfg.getParameter<VPSet>("mapping");
  for ( VPSet::const_iterator mappingEntry = mapping.begin();
	mappingEntry != mapping.end(); ++mappingEntry ) {
    unsigned category = mappingEntry->getParameter<uint32_t>("category");
    std::unique_ptr<DiscriminantCutEntry> cut{new DiscriminantCutEntry()};
    if ( mappingEntry->existsAs<double>("cut") ) {
      cut->cutValue_ = mappingEntry->getParameter<double>("cut");
      cut->mode_ = DiscriminantCutEntry::kFixedCut;
    } else if ( mappingEntry->existsAs<std::string>("cut") ) {
      cut->cutName_ = mappingEntry->getParameter<std::string>("cut");
      std::string cutVariable_string = mappingEntry->getParameter<std::string>("variable");
      cut->cutVariable_.reset( new StringObjectFunction<reco::PFTau>(cutVariable_string) );
      cut->mode_ = DiscriminantCutEntry::kVariableCut;
    } else {
      throw cms::Exception("RecoTauDiscriminantCutMultiplexer")
        << " Undefined Configuration Parameter 'cut' !!\n";
    }
    cuts_[category] = std::move(cut);
  }

  verbosity_ = cfg.getParameter<int>("verbosity");
  if(verbosity_) std::cout << "constructed " << moduleLabel_ << std::endl;
}

RecoTauDiscriminantCutMultiplexer::~RecoTauDiscriminantCutMultiplexer()
{
}

void RecoTauDiscriminantCutMultiplexer::beginEvent(const edm::Event& evt, const edm::EventSetup& es)
{
  if(verbosity_) std::cout << " begin! " << moduleLabel_ << " " << isInitialized_ << std::endl;
  if ( !isInitialized_ ) {
    //Only open the file once and we can close it when this routine is done
    // since all objects gotten from the file will have been copied
    std::unique_ptr<TFile> inputFile;
    if ( !mvaOutputNormalizationName_.empty() ) {
      if ( !loadMVAfromDB_ ) {
	inputFile = openInputFile(inputFileName_);
	mvaOutput_normalization_ = loadObjectFromFile<TFormula>(*inputFile, mvaOutputNormalizationName_);
      } else {
	auto temp = loadTFormulaFromDB(es, mvaOutputNormalizationName_, Form("%s_mvaOutput_normalization", moduleLabel_.data()), verbosity_);
	mvaOutput_normalization_ = std::move(temp);
      }
    }
    for ( DiscriminantCutMap::iterator cut = cuts_.begin();
	  cut != cuts_.end(); ++cut ) {
      if ( cut->second->mode_ == DiscriminantCutEntry::kVariableCut ) {
	if ( !loadMVAfromDB_ ) {
	  if(not inputFile) {
	    inputFile = openInputFile(inputFileName_);
	  }
	  if(verbosity_) std::cout << "Loading from file" << inputFileName_ << std::endl;
	  cut->second->cutFunction_ = loadObjectFromFile<TGraph>(*inputFile, cut->second->cutName_);
	} else {
	  if(verbosity_) std::cout << "Loading from DB" << std::endl;
	  cut->second->cutFunction_ = loadTGraphFromDB(es, cut->second->cutName_, verbosity_);
	}
      }
    }
    isInitialized_ = true;
  }

  evt.getByToken(toMultiplex_token, toMultiplexHandle_);
  evt.getByToken(key_token, keyHandle_);
}

double
RecoTauDiscriminantCutMultiplexer::discriminate(const reco::PFTauRef& tau) const
{
  if ( verbosity_ ) {
    std::cout << "<RecoTauDiscriminantCutMultiplexer::discriminate>:" << std::endl;
    std::cout << " moduleLabel = " << moduleLabel_ << std::endl;
  }

  double disc_result = (*toMultiplexHandle_)[tau];
  if ( verbosity_ ) {
    std::cout << "disc_result = " <<  disc_result << std::endl;
  }
  if ( mvaOutput_normalization_ ) {
    disc_result = mvaOutput_normalization_->Eval(disc_result);
    //if ( disc_result > 1. ) disc_result = 1.;
    //if ( disc_result < 0. ) disc_result = 0.;
    if ( verbosity_ ) {
      std::cout << "disc_result (normalized) = " <<  disc_result << std::endl;
    }
  }
  double key_result = (*keyHandle_)[tau];
  DiscriminantCutMap::const_iterator cutIter = cuts_.find(TMath::Nint(key_result));

  // Return null if it doesn't exist
  if ( cutIter == cuts_.end() ) {
    return prediscriminantFailValue_;
  }
  // See if the discriminator passes our cuts
  bool passesCuts = false;
  if ( cutIter->second->mode_ == DiscriminantCutEntry::kFixedCut ) {
    passesCuts = (disc_result > cutIter->second->cutValue_);
    if ( verbosity_ ) {
      std::cout << "cutValue (fixed) = " << cutIter->second->cutValue_ << " --> passesCuts = " << passesCuts << std::endl;
    }
  } else if ( cutIter->second->mode_ == DiscriminantCutEntry::kVariableCut ) {
    double cutVariable = (*cutIter->second->cutVariable_)(*tau);
    double xMin, xMax, dummy;
    cutIter->second->cutFunction_->GetPoint(0, xMin, dummy);
    cutIter->second->cutFunction_->GetPoint(cutIter->second->cutFunction_->GetN() - 1, xMax, dummy);
    const double epsilon = 1.e-3;
    if      ( cutVariable < (xMin + epsilon) ) cutVariable = xMin + epsilon;
    else if ( cutVariable > (xMax - epsilon) ) cutVariable = xMax - epsilon;
    double cutValue = cutIter->second->cutFunction_->Eval(cutVariable);
    passesCuts = (disc_result > cutValue);
    if ( verbosity_ ) {
      std::cout << "cutValue (@" << cutVariable << ") = " << cutValue << " --> passesCuts = " << passesCuts << std::endl;
    }
  } else assert(0);

  return passesCuts;
}

void
RecoTauDiscriminantCutMultiplexer::fillDescriptions(edm::ConfigurationDescriptions& descriptions) {
  // recoTauDiscriminantCutMultiplexer
  edm::ParameterSetDescription desc;
  desc.add<edm::InputTag>("toMultiplex", edm::InputTag("fixme"));
  desc.add<int>("verbosity", 0);

  {
    edm::ParameterSet pset_mapping;
    pset_mapping.addParameter<unsigned int>("category",0);
    pset_mapping.addParameter<double>("cut",0.);
    edm::ParameterSetDescription desc_mapping;
    desc_mapping.add<unsigned int>("category",0);
    desc_mapping.addNode(edm::ParameterDescription<std::string>("cut", true) xor
                         edm::ParameterDescription<double>("cut", true));
    // it seems the parameter string "variable" exists only when "cut" is string
    // see hpsPFTauDiscriminationByVLooseIsolationMVArun2v1DBdR03oldDMwLT in RecoTauTag/Configuration/python/HPSPFTaus_cff.py
    desc_mapping.addOptional<std::string>("variable")->setComment("the parameter is required when \"cut\" is string");
    //  desc_mapping.add<double>("cut",0.);
    std::vector<edm::ParameterSet> vpsd_mapping;
    vpsd_mapping.push_back(pset_mapping);
    desc.addVPSet("mapping",desc_mapping,vpsd_mapping);
  }

  desc.add<edm::FileInPath>("inputFileName", edm::FileInPath("RecoTauTag/RecoTau/data/emptyMVAinputFile"));
  desc.add<bool>("loadMVAfromDB", true);
  fillProducerDescriptions(desc); // inherited from the base
  desc.add<std::string>("mvaOutput_normalization", "");
  desc.add<edm::InputTag>("key", edm::InputTag("fixme"));
  descriptions.add("recoTauDiscriminantCutMultiplexerDefault", desc);
}

DEFINE_FWK_MODULE(RecoTauDiscriminantCutMultiplexer);


/**
 * @file budgeted_maximum_coverage.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2014-03-18
 */
#ifndef PAAL_BUDGETED_MAXIMUM_COVERAGE_HPP
#define PAAL_BUDGETED_MAXIMUM_COVERAGE_HPP

#include "paal/data_structures/fraction.hpp"
#include "paal/utils/accumulate_functors.hpp"
#include "paal/utils/algorithms/subset_backtrack.hpp"
#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"

#include <boost/heap/d_ary_heap.hpp>
#include <boost/range/adaptor/indexed.hpp>
#include <boost/range/algorithm/copy.hpp>
#include <boost/range/algorithm_ext/iota.hpp>
#include <boost/range/algorithm/fill.hpp>
#include <boost/range/algorithm/for_each.hpp>
#include <boost/range/algorithm/sort.hpp>
#include <boost/range/combine.hpp>

#include <algorithm>
#include <vector>

namespace paal {
namespace greedy {
namespace detail {

template <typename ElementWeight, typename SetCost> struct set_data_type {
    set_data_type()
        : m_weight_of_uncovered_elements{}, m_cost{}, m_is_processed{ true } {}
    ElementWeight m_weight_of_uncovered_elements; // sum of weight of uncovered
                                                  // elements in backtrack and
                                                  // greedy
    SetCost m_cost;
    bool m_is_processed;
    // set is processed if is selected or is not selected and at least one of
    // the following situations occurs
    // -we have not enough budget to select set
    // -sum of weight of uncovered elements belong to set equal 0
};

template <typename SetReference, typename GetElementsOfSet,
          typename GetWeightOfElement>
using element_weight_t = pure_result_of_t<GetWeightOfElement(
    range_to_elem_t<pure_result_of_t<GetElementsOfSet(SetReference)>>)>;

const int UNCOVERED = -1;
template <class Budget, class SetCost, class SetIdToData, class ElementWeight,
          class SetIdToElements, class ElementIndex, class GetWeightOfElement,
          typename DecreseWeight>
class selector {
    const Budget m_budget;
    SetCost &m_cost_of_solution;
    std::vector<int> m_selected_sets;
    SetIdToData &m_sets_data;
    const ElementWeight &m_weight_of_bests_solution;
    ElementWeight &m_weight_of_covered_elements;
    SetIdToElements m_set_id_to_elements;
    std::vector<int> &m_covered_by;
    std::vector<std::vector<int>> &m_sets_covering_element;
    ElementIndex &m_get_el_index;
    GetWeightOfElement &m_element_to_weight;
    DecreseWeight m_decrese_weight;

    using SetData = range_to_elem_t<SetIdToData>;

  public:
    selector(const Budget budget, SetCost &cost_of_solution,
             SetIdToData &sets_data,
             const ElementWeight &weight_of_bests_solution,
             ElementWeight &weight_of_covered_elements,
             SetIdToElements set_id_to_elements, std::vector<int> &covered_by,
             std::vector<std::vector<int>> &sets_covering_element,
             ElementIndex &get_el_index, GetWeightOfElement &element_to_weight,
             DecreseWeight decrese_weight)
        : m_budget(budget), m_cost_of_solution(cost_of_solution),
          m_sets_data(sets_data),
          m_weight_of_bests_solution(weight_of_bests_solution),
          m_weight_of_covered_elements(weight_of_covered_elements),
          m_set_id_to_elements(set_id_to_elements), m_covered_by(covered_by),
          m_sets_covering_element(sets_covering_element),
          m_get_el_index(get_el_index), m_element_to_weight(element_to_weight),
          m_decrese_weight(decrese_weight) {}

    // we return true if (we select set) or (set is already in solution)
    bool select_set_backtrack(int selected_set_id, bool in_reset = false) {
        if(!can_select(m_sets_data[selected_set_id])) return false;

        select_set(selected_set_id, true);
        if(!in_reset) m_selected_sets.push_back(selected_set_id);
        return true;
    }

    // we return true if we violated budget
    bool select_set_greedy(int selected_set_id) {
        auto &select_set_data = m_sets_data[selected_set_id];

        if(!can_select(select_set_data)) return false;
        m_selected_sets.push_back(selected_set_id);

        if(greedy_prune(select_set_data))  return true;
        select_set(selected_set_id, false);
        return false;
    }

    void deselect_set(int selected_set_id, bool backtrack = true) {
        // we deselect set

        m_cost_of_solution -= m_sets_data[selected_set_id].m_cost;
        for (auto element : m_set_id_to_elements(selected_set_id)) {
            if (covered_by(element) == selected_set_id) {
                m_covered_by[m_get_el_index(element)] = UNCOVERED;
                auto weight_of_element = m_element_to_weight(element);
                cover_element(element, -weight_of_element, backtrack, false);
            }
        }
        m_selected_sets.pop_back();
    }

    void set_unprocessed(
            boost::iterator_range<std::vector<int>::const_iterator> const & set_ids) {
        for (auto set_id : set_ids) {
            m_sets_data[set_id].m_is_processed = false;
        }
    };

    void reset() {
        set_unprocessed(m_selected_sets);
        if (m_selected_sets.size() > 0) {
            boost::for_each(m_selected_sets, [&](int selected_set_id) {
                select_set_backtrack(selected_set_id, true);
            });
        }
    }

    std::size_t size() {
        return m_selected_sets.size();
    }

    void resize(std::size_t size) {
        return m_selected_sets.resize(size);
    }

    template <typename OutputIterator>
    void copy_to(OutputIterator out){
        boost::copy(m_selected_sets, out);
    }
private:

    void select_set(int selected_set_id, bool backtrack) {
        auto &select_set_data = m_sets_data[selected_set_id];

        m_cost_of_solution += select_set_data.m_cost;
        for (auto element : m_set_id_to_elements(selected_set_id)) {
            if (covered_by(element) == UNCOVERED) { /* we do not cover the
                                                        elements being covered*/

                m_covered_by[m_get_el_index(element)] = selected_set_id;
                auto weight_of_element = m_element_to_weight(element);
                cover_element(element, weight_of_element, backtrack);
            }
        }
    }

    /// optimization:
    /// in greedy phase we get sets in decreasing density order, so we can cut
    /// when spend rest of budget with current density product solution, worse
    /// then best found solution.
    bool greedy_prune(const SetData & set_data) {
        return (m_budget - m_cost_of_solution) * set_data.m_weight_of_uncovered_elements <=
                static_cast<Budget>(set_data.m_cost * (m_weight_of_bests_solution - m_weight_of_covered_elements));
    }

    ///this function is ALWAYS called from select_set, thats why we set set_data.m_is_processed!
    bool can_select(SetData & set_data) {
        if (set_data.m_is_processed) return false;
        set_data.m_is_processed = true;
        return static_cast<Budget>(m_cost_of_solution + set_data.m_cost) <= m_budget &&
            set_data.m_weight_of_uncovered_elements!= ElementWeight{};
    }

    template <typename Element>
    void cover_element(Element && el, ElementWeight weight_diff, bool backtrack, bool select = true) {
        m_weight_of_covered_elements += weight_diff;
        for (auto set_id :
                m_sets_covering_element[m_get_el_index(el)]) {
            if (m_sets_data[set_id].m_weight_of_uncovered_elements >
                    weight_diff || backtrack) {
                m_sets_data[set_id].m_weight_of_uncovered_elements -= weight_diff;
                if (!backtrack && !m_sets_data[set_id].m_is_processed) {
                    m_decrese_weight(set_id);
                }
            } else {
                if (select) {
                    m_sets_data[set_id].m_is_processed = true;
                }
            }
        }
    }

    template <typename Element>
    auto covered_by(Element && el) const
        -> decltype(m_covered_by[m_get_el_index(el)]) {

        return m_covered_by[m_get_el_index(el)];
    }
};

template <class Budget, class SetCost, class SetIdToData, class ElementWeight,
         class SetIdToElements, class ElementIndex, class GetWeightOfElement,
         typename DecreseWeight>
auto make_selector(const Budget budget, SetCost &cost_of_solution,
                   SetIdToData &sets_data,
                   const ElementWeight &weight_of_bests_solution,
                   ElementWeight &weight_of_covered_elements,
                   SetIdToElements set_id_to_elements,
                   std::vector<int> &covered_by,
                   std::vector<std::vector<int>> &sets_covering_element,
                   ElementIndex &get_el_index,
                   GetWeightOfElement &element_to_weight,
                   DecreseWeight decrese_weight) {
    return selector<Budget, SetCost, SetIdToData, ElementWeight,
                    SetIdToElements, ElementIndex, GetWeightOfElement,
                    DecreseWeight>(
        budget, cost_of_solution, sets_data,
        weight_of_bests_solution, weight_of_covered_elements,
        set_id_to_elements, covered_by, sets_covering_element, get_el_index,
        element_to_weight, decrese_weight);
}
} //!detail

/**
 * @brief this is solve Set Cover problem
 * and return set cover cost
 * example:
 *  \snippet set_cover_example.cpp Set Cover Example
 *
 * complete example is set_cover_example.cpp
 * @param sets
 * @param set_to_cost
 * @param set_to_elements
 * @param result set iterators of chosen sets
 * @param get_el_index
 * @param budget
 * @param element_to_weight
 * @param initial_set_size
 * @tparam SetRange
 * @tparam GetCostOfSet
 * @tparam GetElementsOfSet
 * @tparam OutputIterator
 * @tparam ElementIndex
 * @tparam Budget
 * @tparam GetWeightOfElement
 */
template <typename SetRange, class GetCostOfSet, class GetElementsOfSet,
          class OutputIterator, class ElementIndex, class Budget,
          class GetWeightOfElement = utils::return_one_functor>
auto budgeted_maximum_coverage(
    SetRange && sets, GetCostOfSet set_to_cost,
    GetElementsOfSet set_to_elements, OutputIterator result,
    ElementIndex get_el_index, Budget budget,
    GetWeightOfElement element_to_weight = GetWeightOfElement(),
    const unsigned int initial_set_size = 3) {

    using set_reference = typename boost::range_reference<SetRange>::type;
    using element_weight = typename detail::element_weight_t<
        set_reference, GetElementsOfSet, GetWeightOfElement>;
    using set_cost = pure_result_of_t<GetCostOfSet(set_reference)>;
    using set_id_to_data = std::vector<detail::set_data_type<element_weight, set_cost>>;

    auto nu_sets = boost::distance(sets);
    set_id_to_data initial_sets_data(nu_sets), sets_data(nu_sets);
    set_cost cost_of_solution{}, cost_of_best_solution{};
    int number_of_elements = 0;

    // we find max index of elements in all sets
    for (auto set_and_data : boost::combine(sets, initial_sets_data)) {
        auto const & set = boost::get<0>(set_and_data);
        auto &cost = boost::get<1>(set_and_data).m_cost;
        cost = set_to_cost(set);
        assert(cost != set_cost{});
        auto const & elements = set_to_elements(set);
        if (!boost::empty(elements)) {
            number_of_elements = std::max(number_of_elements,
                    *max_element_functor(elements, get_el_index) + 1);
        }
    }
    element_weight weight_of_covered_elements{}, weight_of_bests_solution{};
    std::vector<int> best_solution(1);
    std::vector<int> covered_by(
        number_of_elements, detail::UNCOVERED); // index of the first set that covers
                                        // element or -1 if element is uncovered
    std::vector<int> sets_id(nu_sets);
    boost::iota(sets_id, 0);
    std::vector<std::vector<int>> sets_covering_element(number_of_elements);
    auto decreasing_density_order =
        utils::make_functor_to_comparator([&](int x) {
            return data_structures::make_fraction(
                sets_data[x].m_weight_of_uncovered_elements, sets_data[x].m_cost);
        });
    using queue = boost::heap::d_ary_heap<
        int, boost::heap::arity<3>, boost::heap::mutable_<true>,
        boost::heap::compare<decltype(decreasing_density_order)>>;

    queue uncovered_set_queue{ decreasing_density_order };
    std::vector<typename queue::handle_type> set_id_to_handle(nu_sets);

    // we fill sets_covering_element and setToWeightOfElements
    for (auto set : sets | boost::adaptors::indexed()) {
        auto set_id = set.index();
        auto &set_data = initial_sets_data[set_id];

        for (auto &&element : set_to_elements(set.value())) {
            sets_covering_element[get_el_index(element)].push_back(set_id);
            set_data.m_weight_of_uncovered_elements += element_to_weight(element);
        }
        if (initial_set_size ==
            0) { /* we check all one element set. if initial_set_size!= 0 then
                    we will do it anyway */
            set_cost cost_of_set = set_data.m_cost;
            if (set_data.m_weight_of_uncovered_elements >= weight_of_bests_solution &&
                static_cast<Budget>(cost_of_set) <= budget) {
                weight_of_bests_solution = set_data.m_weight_of_uncovered_elements;
                best_solution[0] = set_id;
                cost_of_best_solution = cost_of_set;
            }
        }
    };

    auto sort_sets = [&](std::vector<int>& sets_range) {
        boost::sort(sets_range, utils::make_functor_to_comparator([&](int x) {
                                    return sets_data[x].m_weight_of_uncovered_elements;
                                }));
        return sets_range.end();
    };
    auto selector = detail::make_selector
                (budget, cost_of_solution,sets_data,
                 weight_of_bests_solution,weight_of_covered_elements,
                 [&](int selected_set_id){return set_to_elements(sets[selected_set_id]);},
                 covered_by,sets_covering_element,get_el_index,
                 element_to_weight,
                 [&](int set_id){uncovered_set_queue.decrease(set_id_to_handle[set_id]);});

    boost::copy(initial_sets_data, sets_data.begin());
    sort_sets(sets_id);
    auto solver = make_subset_backtrack(sets_id);

    auto reset = [&]() {
        boost::copy(initial_sets_data, sets_data.begin());
        cost_of_solution = set_cost{};
        selector.set_unprocessed(solver.get_moves());
        boost::fill(covered_by, detail::UNCOVERED);
        weight_of_covered_elements = element_weight{};
        selector.reset();
    };

    auto on_pop = [&](int deselected_set_id) {
        selector.deselect_set(deselected_set_id);
        selector.set_unprocessed(solver.get_moves());
    };

    auto save_best_solution = [&]() {
        // we check that new solution is better than any previous
        // if is we remember them
        // tricky: either better weight, or equal weight and lower cost
        if (std::make_pair(weight_of_covered_elements, cost_of_best_solution) >
            std::make_pair(weight_of_bests_solution, cost_of_solution)) {
            weight_of_bests_solution = weight_of_covered_elements;
            cost_of_best_solution = cost_of_solution;
            best_solution.resize(selector.size());
            selector.copy_to(best_solution.begin());
        }
    };
    auto greedy_phase = [&]() {
        uncovered_set_queue.clear();
        auto moves = solver.get_moves();
        if(boost::empty(moves)) return;

        for (auto set_id : moves) {
            set_id_to_handle[set_id] = uncovered_set_queue.push(set_id);
        }
        /* we select set with best elements to cost ratio, and add it to the
         * result until all elements are covered*/
        int uncovered_set_id;
        do {
            uncovered_set_id = uncovered_set_queue.top();
            uncovered_set_queue.pop();
        } while (!selector.select_set_greedy(uncovered_set_id) &&
                !uncovered_set_queue.empty());
    };

    auto can_push = [&](int candidate) {
        if (!selector.select_set_backtrack(candidate)) {
            return false;
        }
        if (selector.size() == initial_set_size) {
            greedy_phase();
            save_best_solution();
            selector.resize(initial_set_size-1);
            reset();
            return false;
        } else {
            save_best_solution();
            return true;
        }
    };

    reset();
    if (initial_set_size != 0) { /* if initial_set_size == 0 then we do greedy
    algorithm once starts from empty initial Set.
    Otherwise we starts greedy algorithm from all initial set of size equal
    initial_set_size those for which we have enough budget*/
        solver.solve(can_push, on_pop, sort_sets);
    } else {
        greedy_phase();
        save_best_solution();
    }

    for (auto set_id : best_solution) {
        *result = *(sets.begin() + set_id);
        ++result;
    }
    return weight_of_bests_solution;
};
} //!greedy
} //!paal

#endif /* PAAL_BUDGETED_MAXIMUM_COVERAGE_HPP */


#ifndef OSRM_UTIL_RECTANGLE_HPP
#define OSRM_UTIL_RECTANGLE_HPP

#include "util/coordinate.hpp"
#include "util/coordinate_calculation.hpp"

#include <boost/assert.hpp>

#include <limits>
#include <utility>

#include <cstdint>

namespace osrm
{
namespace util
{

struct RectangleInt2D
{
    RectangleInt2D()
        : min_lon{std::numeric_limits<std::int32_t>::max()},
          max_lon{std::numeric_limits<std::int32_t>::min()},
          min_lat{std::numeric_limits<std::int32_t>::max()},
          max_lat{std::numeric_limits<std::int32_t>::min()}
    {
    }

    RectangleInt2D(FixedLongitude min_lon_,
                   FixedLongitude max_lon_,
                   FixedLatitude min_lat_,
                   FixedLatitude max_lat_)
        : min_lon(min_lon_), max_lon(max_lon_), min_lat(min_lat_), max_lat(max_lat_)
    {
    }

    RectangleInt2D(FloatLongitude min_lon_,
                   FloatLongitude max_lon_,
                   FloatLatitude min_lat_,
                   FloatLatitude max_lat_)
        : min_lon(toFixed(min_lon_)), max_lon(toFixed(max_lon_)), min_lat(toFixed(min_lat_)),
          max_lat(toFixed(max_lat_))
    {
    }

    FixedLongitude min_lon, max_lon;
    FixedLatitude min_lat, max_lat;

    void MergeBoundingBoxes(const RectangleInt2D &other)
    {
        min_lon = std::min(min_lon, other.min_lon);
        max_lon = std::max(max_lon, other.max_lon);
        min_lat = std::min(min_lat, other.min_lat);
        max_lat = std::max(max_lat, other.max_lat);
        BOOST_ASSERT(min_lon != FixedLongitude{std::numeric_limits<std::int32_t>::min()});
        BOOST_ASSERT(min_lat != FixedLatitude{std::numeric_limits<std::int32_t>::min()});
        BOOST_ASSERT(max_lon != FixedLongitude{std::numeric_limits<std::int32_t>::min()});
        BOOST_ASSERT(max_lat != FixedLatitude{std::numeric_limits<std::int32_t>::min()});
    }

    Coordinate Centroid() const
    {
        Coordinate centroid;
        // The coordinates of the midpoints are given by:
        // x = (x1 + x2) /2 and y = (y1 + y2) /2.
        centroid.lon = (min_lon + max_lon) / FixedLongitude{2};
        centroid.lat = (min_lat + max_lat) / FixedLatitude{2};
        return centroid;
    }

    bool Intersects(const RectangleInt2D &other) const
    {
        // Standard box intersection test - check if boxes *don't* overlap,
        // and return the negative of that
        return !(max_lon < other.min_lon || min_lon > other.max_lon || max_lat < other.min_lat ||
                 min_lat > other.max_lat);
    }

    // This code assumes that we are operating in euclidean space!
    // That means if you just put unprojected lat/lon in here you will
    // get invalid results.
    std::uint64_t GetMinSquaredDist(const Coordinate location) const
    {
        const bool is_contained = Contains(location);
        if (is_contained)
        {
            return 0.0f;
        }

        enum Direction
        {
            INVALID = 0,
            NORTH = 1,
            SOUTH = 2,
            EAST = 4,
            NORTH_EAST = 5,
            SOUTH_EAST = 6,
            WEST = 8,
            NORTH_WEST = 9,
            SOUTH_WEST = 10
        };

        Direction d = INVALID;
        if (location.lat > max_lat)
            d = (Direction)(d | NORTH);
        else if (location.lat < min_lat)
            d = (Direction)(d | SOUTH);
        if (location.lon > max_lon)
            d = (Direction)(d | EAST);
        else if (location.lon < min_lon)
            d = (Direction)(d | WEST);

        BOOST_ASSERT(d != INVALID);

        std::uint64_t min_dist = std::numeric_limits<std::uint64_t>::max();
        switch (d)
        {
        case NORTH:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(location.lon, max_lat));
            break;
        case SOUTH:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(location.lon, min_lat));
            break;
        case WEST:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(min_lon, location.lat));
            break;
        case EAST:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(max_lon, location.lat));
            break;
        case NORTH_EAST:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(max_lon, max_lat));
            break;
        case NORTH_WEST:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(min_lon, max_lat));
            break;
        case SOUTH_EAST:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(max_lon, min_lat));
            break;
        case SOUTH_WEST:
            min_dist = coordinate_calculation::squaredEuclideanDistance(
                location, Coordinate(min_lon, min_lat));
            break;
        default:
            break;
        }

        BOOST_ASSERT(min_dist < std::numeric_limits<std::uint64_t>::max());

        return min_dist;
    }

    bool Contains(const Coordinate location) const
    {
        const bool lons_contained = (location.lon >= min_lon) && (location.lon <= max_lon);
        const bool lats_contained = (location.lat >= min_lat) && (location.lat <= max_lat);
        return lons_contained && lats_contained;
    }

    bool IsValid() const
    {
        return min_lon != FixedLongitude{std::numeric_limits<std::int32_t>::max()} &&
               max_lon != FixedLongitude{std::numeric_limits<std::int32_t>::min()} &&
               min_lat != FixedLatitude{std::numeric_limits<std::int32_t>::max()} &&
               max_lat != FixedLatitude{std::numeric_limits<std::int32_t>::min()};
    }
};
} // namespace util
} // namespace osrm

#endif


/*
    Copyright 2013 Adobe
    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*/
/**************************************************************************************************/

#ifndef ADOBE_dancing_links_t_HPP
#define ADOBE_dancing_links_t_HPP

/**************************************************************************************************/

#include <adobe/config.hpp>

#include <vector>

#if ADOBE_STD_SERIALIZATION
#include <iostream>
#endif

#include <boost/function.hpp>
#include <boost/noncopyable.hpp>

#include <adobe/implementation/toroid.hpp>

#define ADOBE_DLX_VERBOSE 0

#if ADOBE_STD_SERIALIZATION && ADOBE_DLX_VERBOSE
#include <adobe/iomanip.hpp>
#endif

/**************************************************************************************************/

namespace adobe {

/**************************************************************************************************/

#ifndef ADOBE_NO_DOCUMENTATION

/**************************************************************************************************/

namespace implementation {

/**************************************************************************************************/

struct do_nothing_callback_t {
    inline void operator()(std::size_t, bool) const {}
};

/**************************************************************************************************/

struct select_right_heuristic_t {
    inline toroid_header_t* operator()(byte_toroid_t& toroid) const {
        return toroid.right_of(&toroid.header_m);
    }
};

/**************************************************************************************************/

struct select_most_constrained_heuristic_t {
    inline toroid_header_t* operator()(byte_toroid_t& toroid) const {
        toroid_header_t* c(toroid.right_of(&toroid.header_m));
        std::size_t sz(c->size_m);

        for (toroid_header_t* p(toroid.right_of(c)); p != &toroid.header_m;
             p = toroid.right_of(p)) {
            if (p->size_m < sz) {
                c = p;
                sz = p->size_m;
            }

            if (sz == 1)
                break;
        }

        return c;
    }
};

/**************************************************************************************************/

} // namespace implementation

/**************************************************************************************************/

#endif

/**************************************************************************************************/

class dancing_links_t : boost::noncopyable {
public:
#ifndef ADOBE_NO_DOCUMENTATION
    dancing_links_t(std::size_t row_count, std::size_t column_count)
        : toroid_m(row_count, column_count), output_m(column_count, 0), solutions_m(0)
#if ADOBE_DLX_VERBOSE
          ,
          tab_count_m(0)
#endif
    {
    }
#endif

    inline void set(std::size_t row, std::size_t col, char color = 0) {
        toroid_m.set(row, col, color);
    }

    inline void set_secondary_column(std::size_t col) { toroid_m.set_secondary_column(col); }

    template <typename ResultCallback, typename SearchHeuristic>
    inline std::size_t search(std::size_t max_solutions, ResultCallback callback,
                              SearchHeuristic heuristic) {
        max_solutions_m = max_solutions;

        toroid_m.finalize();

        do_search(0, callback, heuristic);

        return solutions_m;
    }

    inline std::size_t search(std::size_t max_solutions) {
        return search(max_solutions, implementation::do_nothing_callback_t(),
                      implementation::select_most_constrained_heuristic_t());
    }

#if ADOBE_STD_SERIALIZATION
    friend std::ostream& operator<<(std::ostream& s, const dancing_links_t& dancing_links_t) {
        return s << dancing_links_t.toroid_m;
    }
#endif

private:
    template <typename ResultCallback, typename SearchHeuristic>
    void do_search(std::size_t k, ResultCallback callback, SearchHeuristic heuristic) {
        if (toroid_m.right_of(&toroid_m.header_m) == &toroid_m.header_m) {
            ++solutions_m;

#if ADOBE_DLX_VERBOSE
            std::cout << adobe::indents(tab_count_m) << "<solved/>" << std::endl;
#endif

            for (std::size_t i(0); i < k; ++i)
                callback(toroid_m.row_index_of(output_m[i]), i + 1 == k);

            return;
        }

        std::size_t next_k(k + 1);

        toroid_header_t* c(heuristic(toroid_m));

#if ADOBE_DLX_VERBOSE
        ++tab_count_m;
        std::cout << adobe::indents(tab_count_m) << "<c"
                  << toroid_m.column_index_of(toroid_m.down_of(c)) << ">" << std::endl;
#endif

        toroid_m.cover_column(c);

        // branch on each node in this column
        for (toroid_node_t* r(toroid_m.down_of(c)); r != c; r = toroid_m.down_of(r)) {
#if ADOBE_DLX_VERBOSE
            std::cout << adobe::indents(tab_count_m) << "<r" << toroid_m.row_index_of(r) << ">"
                      << std::endl;
#endif

            output_m[k] = r;

            // cover or purify each node on the same row as the node we're branching on
            for (toroid_node_t* j(toroid_m.right_of(r)); j != r; j = toroid_m.right_of(j)) {
                if (j->color_m == 0) {
#if ADOBE_DLX_VERBOSE
                    std::cout << adobe::indents(tab_count_m) << "<c" << toroid_m.column_index_of(j)
                              << ">" << std::endl;
#endif
                    toroid_m.cover_column(toroid_m.column_of(j));
                } else if (j->color_m > 0) {
#if ADOBE_DLX_VERBOSE
                    std::cout << adobe::indents(tab_count_m) << "<p" << toroid_m.column_index_of(j)
                              << ">" << std::endl;
#endif
                    toroid_m.purify(j);
                }
            }

#if ADOBE_DLX_VERBOSE
// std::cout << *this << std::endl;
#endif

            do_search(next_k, callback, heuristic);

            if (solutions_m >= max_solutions_m)
                return;

            r = output_m[k];

            c = toroid_m.column_of(r);

            // undo the cover/purify
            for (toroid_node_t* j(toroid_m.left_of(r)); j != r; j = toroid_m.left_of(j)) {
                if (j->color_m == 0) {
#if ADOBE_DLX_VERBOSE
                    std::cout << adobe::indents(tab_count_m) << "</c" << toroid_m.column_index_of(j)
                              << ">" << std::endl;
#endif
                    toroid_m.uncover_column(toroid_m.column_of(j));
                } else if (j->color_m > 0) {
#if ADOBE_DLX_VERBOSE
                    std::cout << adobe::indents(tab_count_m) << "</p" << toroid_m.column_index_of(j)
                              << ">" << std::endl;
#endif
                    toroid_m.unpurify(j);
                }
            }

#if ADOBE_DLX_VERBOSE
            std::cout << adobe::indents(tab_count_m) << "</r" << toroid_m.row_index_of(r) << ">"
                      << std::endl;
#endif
        }

        toroid_m.uncover_column(c);

#if ADOBE_DLX_VERBOSE
        std::cout << adobe::indents(tab_count_m) << "</c"
                  << toroid_m.column_index_of(toroid_m.down_of(c)) << ">" << std::endl;
        --tab_count_m;
#endif
    };

    byte_toroid_t toroid_m;
    std::vector<toroid_node_t*> output_m;
    std::size_t solutions_m;
    std::size_t max_solutions_m;
#if ADOBE_DLX_VERBOSE
    std::size_t tab_count_m;
#endif
};

/**************************************************************************************************/

} // namespace adobe

/**************************************************************************************************/

#endif

/**************************************************************************************************/


/*
 * Luca Anzalone
 */

#include <jni.h>

#include <string>
#include <vector>
#include <mutex>

#include <android/log.h>

#include <opencv2/opencv.hpp>
#include <opencv2/features2d.hpp>  // FAST
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/video/tracking.hpp>

#include <dlib/image_io.h>
#include <dlib/image_processing.h>
#include <dlib/image_processing/generic_image.h>
#include <dlib/image_processing/frontal_face_detector.h>
#include <dlib/opencv/cv_image.h>

#define LOG_TAG "native-lib"
#define LOGD(...) \
  ((void)__android_log_print(ANDROID_LOG_DEBUG, LOG_TAG, __VA_ARGS__))

#define JNI_METHOD(NAME) \
    Java_com_dev_anzalone_luca_facelandmarks_Native_##NAME

#define KERNEL_SIZE 5 // 3, 5, 7, 9

#define NV21 17
#define YV12 842094169
#define YUV_420_888 35
#define PYRAMIDS 3
#define MAX_FRAME_COUNT 5

using namespace std;

// global variables:
dlib::shape_predictor shape_predictor;
std::mutex _mutex;
int imageFormat = NV21;

// -------------------------------------------------------------------------------------------------
// -- Lucas-Kanade Optical Flow Tracker
// -------------------------------------------------------------------------------------------------
namespace LK {
    // variables
    int frameCount = 0;
    bool isTracking = false;
    cv::Mat prev_img;
    vector<cv::Point2f> prev_pts;
    vector<cv::Point2f> next_pts;
    cv::TermCriteria criteria(cv::TermCriteria::COUNT | cv::TermCriteria::EPS, 25, 0.01);
    cv::Size ROI(20, 20);

    /** Initialize tracking with the current frame and detected landmarks */
    void start(cv::Mat &mat, dlib::full_object_detection &pts) {
        // release stuff..
        prev_img.release();
        prev_img = mat;
        prev_pts.clear();
        next_pts.clear();

        // consider the new points
        for (unsigned long i = 0; i < pts.num_parts(); i++) {
            auto pt = pts.part(i);
            prev_pts.push_back(cv::Point2f(pt.x(), pt.y()));
        }

        // reset count
        frameCount = 0;
        isTracking = true;
    }

    /** tracking points in the next captured frame */
    vector<cv::Point2f> track(cv::Mat &frame) {
        vector<uchar> status;
        vector<float> err;
        vector<cv::Point2f> tracked;

        // get the new points from the old one
        calcOpticalFlowPyrLK(prev_img, frame, prev_pts, next_pts, status, err,
                             ROI, PYRAMIDS, criteria);

        for (int i = 0; i < status.size(); ++i) {
            if (status[i] == 0) {
                // flow not found: take the old point
                tracked.push_back(prev_pts[i]);
            } else {
                // flow found: take the new point
                tracked.push_back(next_pts[i]);
            }
        }

        // switch the previous points and image with the current
        swap(prev_img, frame);
        swap(prev_pts, tracked);
        next_pts.clear();

        // increase tracking frame count
        if (frameCount++ > MAX_FRAME_COUNT) {
            isTracking = false;
        }

        return prev_pts;
    }
}
// -------------------------------------------------------------------------------------------------

extern "C"
JNIEXPORT void JNICALL
JNI_METHOD(loadModel)(JNIEnv* env, jclass, jstring detectorPath) {
    try {
        const char *path = env->GetStringUTFChars(detectorPath, JNI_FALSE);

        _mutex.lock();
            // cause the later initialization of the tracking
            LK::isTracking = false;

            // load the shape predictor
            dlib::deserialize(path) >> shape_predictor;
        _mutex.unlock();

        env->ReleaseStringUTFChars(detectorPath, path); //free mem
        LOGD("JNI: model loaded");

    } catch (dlib::serialization_error &e) {
        LOGD("JNI: failed to model -> %s", e.what());
    }
}
//--------------------------------------------------------------------------------------------------

void rotateMat(cv::Mat &mat, int rotation) {
    if (rotation == 90) { // portrait
//        LOGD("JNI: rotation 90");
        cv::transpose(mat, mat);
        cv::flip(mat, mat, -1);
    } else if (rotation == 0) { // landscape-left
//        LOGD("JNI: rotation 0");
        cv::flip(mat, mat, 1);
    } else if (rotation == 180) { // landscape-right
//        LOGD("JNI: rotation 180");
        cv::flip(mat, mat, 0);
    }
}

extern "C"
JNIEXPORT void JNICALL
JNI_METHOD(setImageFormat)(JNIEnv* env, jclass, jint format) {
    imageFormat = format;
}

//--------------------------------------------------------------------------------------------------
//-- LANDMARK DETECTION
//--------------------------------------------------------------------------------------------------
extern "C"
JNIEXPORT jlongArray JNICALL
JNI_METHOD(detectLandmarks)(JNIEnv* env, jclass, jbyteArray yuvFrame, jint rotation, jint width, jint height, jint left, jint top, jint right, jint bottom) {
//    LOGD("JNI: detectLandmarks");

    // copy content of frame into image
    jbyte *data = env->GetByteArrayElements(yuvFrame, 0);

    // convert yuv-frame to cv::Mat
    cv::Mat yuvMat(height + height / 2, width, CV_8UC1, (unsigned char *) data);
    cv::Mat grayMat(height, width, CV_8UC1);

    // to grayscale
    if (imageFormat == NV21)
        cv::cvtColor(yuvMat, grayMat, cv::COLOR_YUV2GRAY_NV21);  // was CV_YUV2GRAY_NV21
    else if (imageFormat == YV12)
        cv::cvtColor(yuvMat, grayMat, cv::COLOR_YUV2GRAY_YV12);  // was CV_YUV2GRAY_YV12

    // adjust rotation according to phone orientation
    rotateMat(grayMat, rotation);

 //   __android_log_print(ANDROID_LOG_VERBOSE, "D/native-lib",
 //                       "left %d right %d width %d, height %d", left, right, width, grayMat.cols);

    // keeping the face inside of the grayMat
    if (left < 0) {
        left = 0;
        LOGD("Face out of left");
    }
    if (right > grayMat.cols) {
        right = grayMat.cols-1;
        LOGD("Face out of right");
    }
    if (top < 0) {
        top = 0;
        LOGD("Face out of top");
    }
    if (bottom > grayMat.rows) {
        bottom = grayMat.rows-1;
        LOGD("Face out of bottom");
    }
    // crop face for enhancements
    cv::Rect faceROI(left, top, right - left, bottom - top);
    cv::Mat face = grayMat(faceROI);

    // apply filters
    cv::medianBlur(face, face, KERNEL_SIZE);  // remove noise
    cv::equalizeHist(face, face);  // improve contrast

    if (!LK::isTracking) {
        // -- DETECT LANDMARKS -- //

        // cv::mat to dlib::image
        dlib::cv_image<unsigned char> image(grayMat);

        // detect landmark points
        _mutex.lock();
        dlib::rectangle region(left, top, right, bottom);
        dlib::full_object_detection points = shape_predictor(image, region);
        _mutex.unlock();

        // result
        auto num_points = points.num_parts();
        jsize len = (jsize) (num_points * sizeof(short)); // num_points * 2

        jlong buffer[len];
        jlongArray result = env->NewLongArray(len);

        // copy points in the buffer
        auto k = 0;
        for (unsigned long i = 0l; i < num_points; ++i) {
            dlib::point p = points.part(i);
            buffer[k++] = p.x();
            buffer[k++] = p.y();
        }

        // set the content of buffer into result array
        env->SetLongArrayRegion(result, 0, len, buffer);

        // free mem
        env->ReleaseByteArrayElements(yuvFrame, data, 0);

        // uncomment to enable tracking for the next frames
//        LK::start(grayMat, points);

        return result;

    } else {
        // -- COMPUTE LK-OPTICAL FLOW --
        auto trackedPts = LK::track(grayMat);

        // result
        auto num_points = trackedPts.size();
        jsize len = (jsize) (num_points * sizeof(short)); // num_points * 2

        jlong buffer[len];
        jlongArray result = env->NewLongArray(len);

        // copy tracked points in the buffer
        auto k = 0;
        for (unsigned long i = 0l; i < num_points; ++i) {
            auto p = trackedPts[i];
            buffer[k++] = static_cast<jlong>(p.x);
            buffer[k++] = static_cast<jlong>(p.y);
        }

        // set the content of buffer into result array
        env->SetLongArrayRegion(result, 0, len, buffer);

        // free mem
        env->ReleaseByteArrayElements(yuvFrame, data, 0);

        return result;
    }
}

//--------------------------------------------------------------------------------------------------


// Copyright 2018 Google LLC
//
// 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 <jni.h>
#include <stdint.h>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include "cpp/jni/macros.h"
#include "cpp/sensor_fusion/online_sensor_fusion.h"

#undef JNI_PACKAGE_NAME
#define JNI_PACKAGE_NAME com_google_vr180_capture_motion

#undef JNI_CLASS_NAME
#define JNI_CLASS_NAME SensorFusion

using vr180::OnlineSensorFusion;

extern "C" {

namespace {
static const double kNanoSecondToSecond = 1e-9;

static Eigen::Vector3d JNIFloatArrayToVector3d(JNIEnv* env, jfloatArray array) {
  float tmp[3];
  env->GetFloatArrayRegion(array, 0, 3, tmp);
  const Eigen::Vector3d vec3(tmp[0], tmp[1], tmp[2]);
  return vec3;
}

static Eigen::Matrix3d JNIFloatArrayToMatrix3d(JNIEnv* env, jfloatArray array) {
  float tmp[9];
  env->GetFloatArrayRegion(array, 0, 9, tmp);
  return Eigen::Map<Eigen::Matrix3f>(tmp, 3, 3).cast<double>();
}

inline jlong jptr(OnlineSensorFusion* native_object) {
  return reinterpret_cast<intptr_t>(native_object);
}

inline OnlineSensorFusion* native(jlong ptr) {
  return reinterpret_cast<OnlineSensorFusion*>(ptr);
}

}  // namespace

JNIEXPORT jlong JNICALL JNI_METHOD(nativeInit)(
    JNIEnv* env, jobject obj, jfloatArray device_to_imu_transform) {
  OnlineSensorFusion::Options options;
  options.device_to_imu_transform =
      JNIFloatArrayToMatrix3d(env, device_to_imu_transform);
  return jptr(new OnlineSensorFusion(options));
}

JNIEXPORT void JNICALL JNI_METHOD(nativeRelease)(JNIEnv* env, jobject obj,
                                                 jlong native_object) {
  delete native(native_object);
}

JNIEXPORT void JNICALL JNI_METHOD(nativeAddGyroMeasurement)(
    JNIEnv* env, jobject obj, jlong native_object, jfloatArray gyro,
    jlong timestamp_ns) {
  OnlineSensorFusion* filter = native(native_object);
  if (filter == nullptr) {
    return;
  }
  const auto gyro_vec3 = JNIFloatArrayToVector3d(env, gyro);
  const double timestamp_s = timestamp_ns * kNanoSecondToSecond;
  filter->AddGyroMeasurement(gyro_vec3, timestamp_s);
}

JNIEXPORT void JNICALL JNI_METHOD(nativeAddAccelMeasurement)(
    JNIEnv* env, jobject obj, jlong native_object, jfloatArray accel,
    jlong timestamp_ns) {
  OnlineSensorFusion* filter = native(native_object);
  if (filter == nullptr) {
    return;
  }
  const auto accel_vec3 = JNIFloatArrayToVector3d(env, accel);
  const double timestamp_s = timestamp_ns * kNanoSecondToSecond;
  filter->AddAccelMeasurement(accel_vec3, timestamp_s);
}

JNIEXPORT jfloatArray JNICALL JNI_METHOD(nativeGetOrientation)(
    JNIEnv* env, jobject obj, jlong native_object) {
  OnlineSensorFusion* filter = native(native_object);
  jfloatArray out = env->NewFloatArray(3);
  if (filter == nullptr) {
    return out;
  }
  Eigen::Vector3f v = filter->GetOrientation();
  float carray[3] = {v[0], v[1], v[2]};
  env->SetFloatArrayRegion(out, 0, 3, carray);
  return out;
}

JNIEXPORT void JNICALL JNI_METHOD(nativeRecenter)(JNIEnv* env, jobject obj,
                                                  jlong native_object) {
  OnlineSensorFusion* filter = native(native_object);
  if (filter == nullptr) {
    return;
  }
  filter->Recenter();
}

JNIEXPORT void JNICALL JNI_METHOD(nativeSetGyroBias)(JNIEnv* env, jobject obj,
                                                     jlong native_object,
                                                     jfloatArray bias) {
  OnlineSensorFusion* filter = native(native_object);
  if (filter == nullptr) {
    return;
  }
  const auto gyro_bias = JNIFloatArrayToVector3d(env, bias);
  filter->SetGyroBias(gyro_bias);
}
}


// Copyright (c) 2020 fortiss GmbH
//
// Authors: Julian Bernhard, Klemens Esterle, Patrick Hart and
// Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "bark/commons/params/setter_params.hpp"
#include "bark/geometry/commons.hpp"
#include "bark/geometry/line.hpp"
#include "bark/geometry/polygon.hpp"
#include "bark/models/dynamic/single_track.hpp"
#include "bark/models/behavior/dynamic_model/dynamic_model.hpp"
#include "bark/models/dynamic/single_track.hpp"
#include "bark/models/dynamic/single_track_steering_rate.hpp"
#include "bark/world/observed_world.hpp"
#include "bark/world/tests/make_test_world.hpp"

using namespace bark::models::dynamic;
using namespace bark::models::execution;
using namespace bark::commons;
using namespace bark::models::behavior;
using namespace bark::models::dynamic;
using namespace bark::world;
using namespace bark::geometry;
using namespace bark::world::tests;
using bark::geometry::standard_shapes::GenerateGoalRectangle;

class DummyObservedWorld : public ObservedWorld {
 public:
  DummyObservedWorld(const State& init_state, const ParamsPtr& params, DynamicModelPtr dyn)
      : ObservedWorld(std::make_shared<World>(params), AgentId(0)),
        init_state_(init_state) {

    auto agent = std::make_shared<Agent>(init_state, nullptr, dyn, nullptr,
                                         Polygon(), params);
    agent->SetAgentId(0);
    AddAgent(agent);
  }

  virtual State CurrentEgoState() const { return init_state_; }

  virtual double GetWorldTime() const { return 0.0; }

 private:
  State init_state_;
};

TEST(behavior_motion_primitives_add, behavior_test) {
  auto params = std::make_shared<SetterParams>();
  BehaviorDynamicModel behavior(params);
  Input u(2);
  u << 0, 0;
  behavior.ActionToBehavior(u);

  // single track model
  DynamicModelPtr dyn(new SingleTrackModel(params));
  State init_state0(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  init_state0 << 0.0, 0.0, 0.0, 0.0, 1.0;
  DummyObservedWorld world0(init_state0, params, dyn);
  behavior.ActionToBehavior(u);
  Trajectory traj0 = behavior.Plan(0.5, world0);
  EXPECT_NEAR(traj0(traj0.rows() - 1, StateDefinition::X_POSITION), 0.5, 0.1);
  EXPECT_NEAR(traj0(traj0.rows() - 1, StateDefinition::Y_POSITION), 0.0, 0.1);

  // test single track steering rate model
  DynamicModelPtr dyn_steering_rate(new SingleTrackSteeringRateModel(params));
  State init_state1(static_cast<int>(StateDefinition::MIN_STATE_SIZE) + 1);
  init_state1 << 0.0, 0.0, 0.0, 0.0, 1.0, 0.0;
  DummyObservedWorld world1(init_state1, params, dyn_steering_rate);
  u << 0.5, 3.; // acceleration and steering-rate
  behavior.ActionToBehavior(u);
  Trajectory traj1 = behavior.Plan(0.5, world1);
  EXPECT_NEAR(traj1(traj1.rows() - 1, StateDefinition::X_POSITION), 0.55, 0.1);
  EXPECT_NEAR(traj1(traj1.rows() - 1, StateDefinition::X_POSITION), 0.52, 0.1);

  // real BARK world
  double ego_velocity = 5.0, rel_distance = 5.0,
        velocity_difference = 4;
  double time_step = 0.02f;
  int num_steps = 1000;
  Polygon polygon = GenerateGoalRectangle(6, 3);
  std::shared_ptr<Polygon> goal_polygon(
    std::dynamic_pointer_cast<Polygon>(polygon.Translate(Point2d(50, -2))));

  auto goal_definition_ptr = std::make_shared<GoalDefinitionPolygon>(
    *goal_polygon);

  WorldPtr test_world = make_test_world(
    1, rel_distance, ego_velocity, velocity_difference, goal_definition_ptr);

  // set steering rate behavior model
  // test_world->GetAgents()[0]->GetBehaviorModel() = behavior;

}

int main(int argc, char** argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}