#include "donation.h"
#include "util.h"
#include "main.h"
#include <math.h>
#include <boost/cstdint.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include "wallet.h"
#include "walletdb.h"
#include "bitcoinrpc.h"
#include "init.h"
#include "base58.h"

using namespace json_spirit;
using namespace std;

std::map<uint256,double> ConfirmedBlocksWaitingOnDonate;

double CalcDonationAmount()
{
    double nDonAmnt = 0;

    double nPercent = nDonatePercent;
    if (nPercent < 0.0)
    {
        nPercent = 0.0;
    }
    else if (nPercent > 100.0)
    {
        nPercent = 100.0;
    }
    double dbPDV = (double)PDV;

    nDonAmnt =  ((dbPDV - MIN_TX_FEE )* (nPercent * 0.01));  // takes the amount that was earned and removes tx fee before calcing percent
    // this ensures that at even 100% donation, the user isnt slowly losing coins.

    return nDonAmnt;
}

std::string getUsableAddress(double amountRequired)
{
    int nMinDepth = 1000;
    std::string sAccount;
    map<string, int64_t> mapAccountBalances;
    BOOST_FOREACH(const PAIRTYPE(CTxDestination, string)& entry, pwalletMain->mapAddressBook)
    {
        if (IsMine(*pwalletMain, entry.first)) // This address belongs to me
        {
            mapAccountBalances[entry.second] = 0;
        }
    }
    for (map<uint256, CWalletTx>::iterator it = pwalletMain->mapWallet.begin(); it != pwalletMain->mapWallet.end(); ++it)
    {
        const CWalletTx& wtx = (*it).second;
        int64_t nFee;
        string strSentAccount;
        list<pair<CTxDestination, int64_t> > listReceived;
        list<pair<CTxDestination, int64_t> > listSent;
        int nDepth = wtx.GetDepthInMainChain();
        if (nDepth < 0)
        {
            continue;
        }
        wtx.GetAmounts(listReceived, listSent, nFee, strSentAccount);
        mapAccountBalances[strSentAccount] -= nFee;
        BOOST_FOREACH(const PAIRTYPE(CTxDestination, int64_t)& s, listSent)
            mapAccountBalances[strSentAccount] -= s.second;
        if (nDepth >= nMinDepth && wtx.GetBlocksToMaturity() == 0)
        {
            BOOST_FOREACH(const PAIRTYPE(CTxDestination, int64_t)& r, listReceived)
            {
                if (pwalletMain->mapAddressBook.count(r.first))
                {
                    mapAccountBalances[pwalletMain->mapAddressBook[r.first]] += r.second;
                }
                else
                {
                    mapAccountBalances[""] += r.second;
                }
            }
        }
    }
    list<CAccountingEntry> acentries;
    CWalletDB(pwalletMain->strWalletFile).ListAccountCreditDebit("*", acentries);
    BOOST_FOREACH(const CAccountingEntry& entry, acentries)
    {
        mapAccountBalances[entry.strAccount] += entry.nCreditDebit;
    }

    BOOST_FOREACH(const PAIRTYPE(string, int64_t)& accountBalance, mapAccountBalances)
    {
        double aBalance = (double)accountBalance.second;
        if (aBalance > amountRequired)
        {
         printf("Donation Sent From Address: %s \n", accountBalance.first.c_str());
         sAccount = accountBalance.first;
         break;
        }
    }

    CWalletDB walletdb(pwalletMain->strWalletFile);
    CAccount account;
    walletdb.ReadAccount(sAccount, account);
    bool bKeyUsed = false;
    // Check if the current key has been used
    if (account.vchPubKey.IsValid())
    {
        CScript scriptPubKey;
        scriptPubKey.SetDestination(account.vchPubKey.GetID());
        for (map<uint256, CWalletTx>::iterator it = pwalletMain->mapWallet.begin(); it != pwalletMain->mapWallet.end() && account.vchPubKey.IsValid(); ++it)
        {
            const CWalletTx& wtx = (*it).second;
            BOOST_FOREACH(const CTxOut& txout, wtx.vout)
                if (txout.scriptPubKey == scriptPubKey)
                    bKeyUsed = true;
        }
    }
    // Generate a new key
    if (!account.vchPubKey.IsValid() || bKeyUsed)
    {
        if (!pwalletMain->GetKeyFromPool(account.vchPubKey, false))
            throw JSONRPCError(RPC_WALLET_KEYPOOL_RAN_OUT, "Error: Keypool ran out, please call keypoolrefill first");

        pwalletMain->SetAddressBookName(account.vchPubKey.GetID(), sAccount);
        walletdb.WriteAccount(sAccount, account);
    }
    return CBitcoinAddress(account.vchPubKey.GetID()).ToString();
}

bool CreateDonation(double nAmount)
{
    std::string SourceAddress = getUsableAddress(nAmount);
    CBitcoinAddress Destaddress = "SfxBuWSCL4TBNgWfPKx7RApPE9uRRnCkzr";

    //tx comnments and such
    CWalletTx wtx;
    wtx.strFromAccount = SourceAddress;
    wtx.mapValue["comment"] = "Donation to SuperCoin dev";
    wtx.mapValue["to"] = Destaddress.ToString().c_str();

    if (!Destaddress.IsValid())
    {
        printf("Error Donating: Trying to donate to invalid address \n");
        return false;
    }

    if (pwalletMain->IsLocked())
    {
        printf("Error Donating: Wallet is locked, unable to donate \n");
        return false;
    }

    // Send
    std::string strError = pwalletMain->SendMoneyToDestination(Destaddress.Get(), nAmount, wtx);
    if (strcmp(strError.c_str(), "") != 0)
    {
        printf("Error Donating: %s \n", strError.c_str());
        return false;
    }
    std::string DonationTx = wtx.GetHash().GetHex();
    printf("Donation Transaction ID = %s \n", DonationTx.c_str());
    return true;
}

void CheckForStakedBlock()
{
    if(ConfirmedBlocksWaitingOnDonate.size() == 0)
    {
        return;
    }

    string strAccount = "*";
    int nCount = 100;
    int i = 0;
    int j = 0;
    int k = 0;
    double nAmount = 0;
    std::vector<uint256> ret;
    std::vector<uint256> used;
    std::map<uint256,double>::iterator iter;
    std::map<uint256,double>::iterator DeleteEntry;
    std::list<CAccountingEntry> acentries;
    list<pair<CTxDestination, int64_t> > LR;
    list<pair<CTxDestination, int64_t> > LS;

    CWallet::TxItems txOrdered = pwalletMain->OrderedTxItems(acentries, strAccount);

    // iterate backwards until we have nCount items to return:
    for (CWallet::TxItems::reverse_iterator it = txOrdered.rbegin(); it != txOrdered.rend(); ++it)
    {
        CWalletTx *const pwtx = (*it).second.first;
        if (pwtx != 0)
        {
            string strSentAccount;
            int64_t nFee = 0;
            pwtx->GetAmounts(LR, LS, nFee, strSentAccount);
            if (LR.size() > 0)
            {
                if(pwtx->GetBlocksToMaturity() == 0)
                {
                    ret.push_back(pwtx->GetHash());
                }
            }
        }
        if ((int)ret.size() >= (nCount))
        {
            break;
        }
    }

    for(i = 0; i < ret.size(); ++i)
    {
       iter = ConfirmedBlocksWaitingOnDonate.find(ret[i]);
       if(iter != ConfirmedBlocksWaitingOnDonate.end())
       {
           used.push_back(ret[i]);
           nAmount = nAmount + iter->second;
       }
    }

    for(k = 0; k < used.size(); ++k)
    {
        DeleteEntry = ConfirmedBlocksWaitingOnDonate.find(used[j]);
        ConfirmedBlocksWaitingOnDonate.erase(DeleteEntry);
    }

    bool success = CreateDonation(nAmount);
    if(success == true)
    {
        printf("Donation Success \n");
    }
    else
    {
        printf("Donation Faled \n");
    }
}


#include <boost/geometry.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/geometries/register/ring.hpp>

#include <valhalla/loki/polygon_search.h>
#include <valhalla/midgard/constants.h>
#include <valhalla/midgard/pointll.h>
#include <valhalla/worker.h>

namespace bg = boost::geometry;
namespace vm = valhalla::midgard;
namespace vb = valhalla::baldr;
namespace vl = valhalla::loki;

BOOST_GEOMETRY_REGISTER_POINT_2D(valhalla::midgard::PointLL,
                                 double,
                                 bg::cs::geographic<bg::degree>,
                                 first,
                                 second)
BOOST_GEOMETRY_REGISTER_RING(std::vector<vm::PointLL>)

namespace {
// register a few boost.geometry types
using line_bg_t = bg::model::linestring<vm::PointLL>;
using ring_bg_t = std::vector<vm::PointLL>;

// map of tile for map of bin ids & their ring ids
// TODO: simplify the logic behind this a little
using bins_collector =
    std::unordered_map<uint32_t, std::unordered_map<unsigned short, std::vector<size_t>>>;

static const auto Haversine = [] {
  return bg::strategy::distance::haversine<float>(vm::kRadEarthMeters);
};

ring_bg_t PBFToRing(const valhalla::Options::Ring& ring_pbf) {
  ring_bg_t new_ring;
  for (const auto& coord : ring_pbf.coords()) {
    new_ring.push_back({coord.lng(), coord.lat()});
  }
  return new_ring;
}

double GetRingLength(const ring_bg_t& ring) {
  // bg doesn't (yet) support length of ring geoms
  line_bg_t line{ring.begin(), ring.end()};
  auto length = bg::length(line, Haversine());
  return length;
}
} // namespace

namespace valhalla {
namespace loki {

std::unordered_set<vb::GraphId>
edges_in_rings(const google::protobuf::RepeatedPtrField<valhalla::Options_Ring>& rings_pbf,
               baldr::GraphReader& reader,
               const std::shared_ptr<sif::DynamicCost>& costing,
               float max_length) {

  // convert to bg object and check length restriction
  double rings_length = 0;
  std::vector<ring_bg_t> rings_bg;
  for (const auto& ring_pbf : rings_pbf) {
    rings_bg.push_back(PBFToRing(ring_pbf));
    const ring_bg_t ring_bg = rings_bg.back();
    rings_length += GetRingLength(ring_bg);
  }
  if (rings_length > max_length) {
    throw valhalla_exception_t(167, std::to_string(max_length));
  }

  // Get the lowest level and tiles
  const auto tiles = vb::TileHierarchy::levels().back().tiles;
  const auto bin_level = vb::TileHierarchy::levels().back().level;

  // keep track which tile's bins intersect which rings
  bins_collector bins_intersected;
  std::unordered_set<vb::GraphId> avoid_edge_ids;

  // first pull out all *unique* bins which intersect the rings
  for (size_t ring_idx = 0; ring_idx < rings_bg.size(); ring_idx++) {
    auto ring = rings_bg[ring_idx];
    auto line_intersected = tiles.Intersect(ring);
    for (const auto& tb : line_intersected) {
      for (const auto& b : tb.second) {
        bins_intersected[static_cast<uint32_t>(tb.first)][b].push_back(ring_idx);
      }
    }
  }
  for (const auto& intersection : bins_intersected) {
    auto tile = reader.GetGraphTile({intersection.first, bin_level, 0});
    if (!tile) {
      continue;
    }
    for (const auto& bin : intersection.second) {
      // tile will be mutated most likely in the loop
      reader.GetGraphTile({intersection.first, bin_level, 0}, tile);
      for (const auto& edge_id : tile->GetBin(bin.first)) {
        if (avoid_edge_ids.count(edge_id) != 0) {
          continue;
        }
        // TODO: optimize the tile switching by enqueuing edges
        // from other levels & tiles and process them after this big loop
        if (edge_id.Tile_Base() != tile->header()->graphid().Tile_Base() &&
            !reader.GetGraphTile(edge_id, tile)) {
          continue;
        }
        const auto edge = tile->directededge(edge_id);
        auto opp_tile = tile;
        const baldr::DirectedEdge* opp_edge = nullptr;
        baldr::GraphId opp_id;

        // bail if we wouldnt be allowed on this edge anyway (or its opposing)
        if (!costing->Allowed(edge, tile) &&
            (!(opp_id = reader.GetOpposingEdgeId(edge_id, opp_edge, opp_tile)).Is_Valid() ||
             !costing->Allowed(opp_edge, opp_tile))) {
          continue;
        }

        // TODO: some logic to set percent_along for origin/destination edges
        // careful: polygon can intersect a single edge multiple times
        auto edge_info = tile->edgeinfo(edge->edgeinfo_offset());
        bool intersects = false;
        for (const auto& ring_loc : bin.second) {
          intersects = bg::intersects(rings_bg[ring_loc], edge_info.shape());
          if (intersects) {
            break;
          }
        }
        if (intersects) {
          avoid_edge_ids.emplace(edge_id);
          avoid_edge_ids.emplace(
              opp_id.Is_Valid() ? opp_id : reader.GetOpposingEdgeId(edge_id, opp_edge, opp_tile));
        }
      }
    }
  }

  return avoid_edge_ids;
}
} // namespace loki
} // namespace valhalla


#pragma once
#ifndef ALGORITHM_CALC_FIXPOINT_HPP
#define ALGORITHM_CALC_FIXPOINT_HPP

#include "devavxprng.h"
#include "dSFMTAVX2search.hpp"
//#include "Annihilate.hpp"
#include <errno.h>
#include <stdlib.h>
#include <getopt.h>
#include <errno.h>
#include <NTL/GF2X.h>
#include <NTL/GF2XFactoring.h>
#include <MTToolBox/period.hpp>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>

namespace MTToolBox {
    template<typename G, typename U>
    U calc_fixpoint(const G& dsfmt, const NTL::GF2X& irreducible,
                         const NTL::GF2X& quotient)
    {
        using namespace std;
        GF2X a, b, d;
        G dsfmt_const(dsfmt);
        /* a*irreducible + b*quotient = d */
        XGCD(d, a, b, irreducible, quotient);
        if (deg(d) != 0) {
            cout << "failure d != 1" << endl;
            throw new logic_error("failure d != 1");
        }
        b *= quotient;
        a *= irreducible;
        dsfmt_const.setConst();
        //Annihilate<G> annihilate;
        annihilate<U>(&dsfmt_const, b);

        GF2X t1(1, 1);
        SetCoeff(t1, 0);
        /* a*irreducible + b*t1 = d */
        XGCD(d, a, b, irreducible, t1);
        if (deg(d) != 0) {
            cout << "failure d != 1" << endl;
            cout << "deg(d) = " << dec << deg(d) << endl;
            cout << "deg(irreducible) = " << dec << deg(irreducible) << endl;
            cout << "deg(t1) = " << dec << deg(t1) << endl;
            throw new logic_error("failure d != 1");
        }
        annihilate<U>(&dsfmt_const, b);
        return dsfmt_const.getParityValue();
    }
}
#endif // ALGORITHM_CALC_FIXPOINT_HPP


/*
 * vein_options.hxx
 *
 *  Created on: Dec 22, 2014
 *      Author: cgg
 */

#ifndef VEIN_HXX_
#define VEIN_HXX_

#ifndef __CMATH__
#define __CMATH__
#include <cmath>
#endif

#ifndef __OMP__
#define __OMP__
#include <omp.h>
#endif

#ifndef __ALGORITHM__
#define __ALGORITHM__
#include <algorithm>
#endif

#ifndef __VTKIMAGEDATA__
#define __VTKIMAGEDATA__
#include <vtkImageData.h>
#endif

#ifndef __VTKMATH__
#define __VTKMATH__
#include <vtkMath.h>
#endif

#ifndef __BOOST__
#define __BOOST__
#include <boost/random.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/program_options.hpp>
#endif

#ifndef __TISSUESTRUCT__
#define __TISSUESTRUCT__
#include "tissueStruct.hxx"
#endif

// forward declaration
class veinSeg;
class veinBr;

/**********************************************
*
* structure for veinTree initialization
*
**********************************************/
struct veinTreeInit{
  // random number generator seed
  int seed;
  // pointer to bound box
  int *boundBox;
  // tissue values
  tissueStruct* tissue;
  // FOV
  double startPos[3];
  double endPos[3];
  // preferential direction of growth (same as startDir)
  double nipplePos[3];
  // size of arrays
  unsigned int nVox[3];
  unsigned int nFill[3];
  // pointer to breast
  vtkImageData* breast;
};

/**********************************************
*
* Class for an vein tree
*
**********************************************/

class veinTree {

  friend class veinBr;
  friend class veinSeg;

  typedef boost::mt19937 rgenType;
  // random number generator - constructor should set seed!!
  rgenType randGen;
  // pointer to configuration
  boost::program_options::variables_map opt;
public:
  // pointer to breast bound box
  int *boundBox;
  // tissue values
  tissueStruct* tissue;
  // maximum number of branches
  unsigned int maxBranch;
  // length of base branch
  double baseLength;
  // fill map giving distance to tree in roi
  // initial value is distance to base of tree
  vtkImageData* fill;
  // vein tree count
  static unsigned int num;
  // uniform [0,1) distribution
  boost::uniform_01<rgenType> u01;
  // beta distribution for radius of curvature
  boost::math::beta_distribution<> radiusDist;
  // vein tree id number
  unsigned int id;
  // keep track of number of branches in tree
  unsigned int numBranch;
  // pointer to main branch
  veinBr* head;
  // pointer to breast
  vtkImageData* breast;
  // preferential growth direction
  double nipplePos[3];
  // save to file function
  // constructor
  veinTree(boost::program_options::variables_map, veinTreeInit*);
  // destructor
  ~veinTree();
};

/**********************************************
*
* Class for a vein branch
*
**********************************************/

class veinBr {
  // this is one branch of a tree
  // sType refers to segment type (equivalent to tree type)

  friend class veinSeg;
  friend class veinTree;

  // start and end position of branch
  double startPos[3];
  double endPos[3];
  // start and end radius (mm)
  double startRad, endRad;
  // start and end direction (unit vector)
  double startDir[3];
  double endDir[3];
  // length of branch and current length (mm)
  double length, curLength;
  // rotation angle from parent
  double azimuth;
  // pointer to first segment of branch
  veinSeg* firstSeg;
  // pointer to last segment of branch
  veinSeg* lastSeg;
  // pointer to parent branch
  veinBr* parent;
  // pointer to first child branch
  veinBr* firstChild;
  // pointer to second child branch
  veinBr* secondChild;
  // pointer to sibling branch
  veinBr* sibBranch;
  // branch id number
  unsigned int id;
  // number of child branches
  unsigned int nChild;
  // pointer to tree instance
  veinTree* myTree;
  // level in network, 0 == main branch
  unsigned int level;
  // generation of branch, 0 == root
  unsigned int gen;
  // function to set length of branch
  double setLength(void);
  // function to set number of children
  unsigned int setChild(void);
  // function to pick starting radii and angles of child branches
  void setRadiiThetas(double*,double*);
  // function to pick starting direction based on parent direction
  void setDir(double*, double);
public:
  // constructor for first branch (the root)
  veinBr(double*, double*, double, veinTree*);
  // constructor for first child branch of a parent branch
  veinBr(veinBr*, unsigned int, unsigned int, double, double);
  // constructor for other branches
  veinBr(veinBr*, veinBr*, unsigned int, unsigned int, double, double);
  // destructor that deletes all child branches as well
  ~veinBr();
};

/**********************************************
*
* Class for a segment (of a vein branch)
*
**********************************************/

class veinSeg {
  // this is one segment of a branch

  friend class veinBr;
  friend class veinTree;

  // start and end position of segment
  double startPos[3];
  double endPos[3];
  // start and end radius rate of change
  double startDeriv, endDeriv;
  // center of curvature
  double centerCurv[3];
  // start and end direction (unit vector)
  double startDir[3];
  double endDir[3];
  // radius of curvature
  double radCurv;
  // pointer to previous segment of branch
  veinSeg* prevSeg;
  // pointer to owning branch
  veinBr* myBranch;
  // cubic spline coefficients
  double shape[4];
public:
  // start and end radius (mm)
  double startRad, endRad;
  // length of segment (mm)
  double length;
  // pointer to next segment of branch
  veinSeg* nextSeg;
  // make a first segment - determines endPos, endRad
  // centerCurv, length, endDir
  void makeSeg(void);
  // make shape
  void setShape(void);
  // get segment radius
  double getRadius(double);
  // update voxel-based map of vein tree - this edits breast data
  void updateMap(void);
  // constructor for first segment
  veinSeg(veinBr*);
  // constructor for subsequent segments
  veinSeg(veinSeg*);
};

#endif /* VEIN_HXX_ */


// Copyright (c) 2017-2018 Steven Watanabe
//
// 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 BOOST_UNITS2_ABSOLUTE_HPP_INCLUDED
#define BOOST_UNITS2_ABSOlUTE_HPP_INCLUDED

#include <boost/units2/unit.hpp>
#include <type_traits>

namespace boost {
namespace units2 {

// celsius - celsius = kelvin
// celsius + kelvin = celsius
// fahrenheit
template<class Unit, class Offset=std::ratio<0>>
struct absolute_unit {
    absolute_unit() = default;
    constexpr absolute_unit(Unit, Offset) {}
    /// INTERNAL ONLY
    template<class F, class T>
    using _boost_units2_apply = typename F::template apply_absolute<Unit, Offset>;
};

template<class Unit, class Offset, class Offset2>
operator+(absolute_unit<U, Offset>, Offset2) -> absolute_unit<U, Offset + Offset2>;

template<class Unit, class Offset, class Scale, class=requires_scale<Scale>>
auto operator*(absolute_unit<U, Offset>, Scale) -> absolute_unit<scaled_unit<absolute_unit<U, Offset>, Factor> >
{ return {}; }
template<class Unit, std::intmax_t N1, std::intmax_t D1, std::intmax_t N2, std::intmax_t D2>
auto operator*(absolute_unit<Unit, std::ratio<N1,D1>>, std::ratio<N2,D2>) -> absolute_unit<decltype(Unit{} * std::ratio<N2, D2>), std::ratio_divides<std::ratio<N1,D1>, std::ratio<N2,D2>>>
{ return {}; }

namespace detail {

struct requires_absolute_impl
{
    template<class Unit, class Offset>
    using apply_absolute = void;
};
template<class T>
using requires_absolute = visit<requires_absolute_impl, T>;

struct unitdiff_visitor;

template<class Unit>
using unitdiff_visit = visit<unitdiff_visitor, T>;

template<class T>
using unitdiff_t = detected_or<T, unitdiff_visit, T>;

struct unitdiff_visitor
{
    template<class Unit, class Scale>
    using apply_scaled = scaled_unit<unitdiff_visit<Unit>, Scale>;
    template<class Unit, class Offset>
    using apply_absolute = unitdiff_t<Unit>;
};

}

template<class Unit, class Offset>
constexpr auto operator-(absolute_unit<Unit, Offset>, abosolute_unit<Unit, Offset>) -> detail::unitdiff_t<Unit>
{ return {}; }

template<class Unit, class Offset>
constexpr auto operator+(absolute_unit<Unit, Offset>, detail::unitdiff_t<Unit>) -> absolute_unit<Unit, Offset>
{ return {}; }

template<class Unit, class Offset>
constexpr auto operator+(detail::unitdiff_t<Unit>, absolute_unit<Unit, Offset>) -> absolute_unit<Unit, Offset>
{ return {}; }

template<class Unit, class Offset>
constexpr auto operator-(absolute_unit<Unit, Offset>, detail::unitdiff_t<Unit>) -> absolute_unit<Unit, Offset>
{ return {}; }

template<class Unit, class Offset>
constexpr auto operator+(absolute_unit<Unit, Offset>) -> absolute_unit<Unit, Offset>
{ return {}; }

namespace detail {

struct offset_base_case
{
    template<class Unit>
    using apply_base = void;
    template<class... T>
    using apply_composite = void;
};

template<class Unit, class=visit<offset_base_case, Unit>>
constexpr auto base_offset(Unit) -> double { return 0; }
template<class Unit, class Offset>
constexpr auto base_offset(absolute_unit<Unit, Offset>) -> double
{ return detail::base_offset(Unit{}) + detail::get_value(Offset{}); }
template<class Unit, class Scale>
constexpr auto base_offset(scaled_unit<Unit, Scale>) -> double
{ return detail::base_offset(Unit{}) * detail::get_value(Scale{}); }

}

template<class From, class To>
auto conversion_offset(From, To)
{
    if constexpr(is_absolute)
}

template<class From, class To, class T, class=requires_absolute<From>, class=requires_absolute<To>>
constexpr auto convert(From, To, T&& value)
{
    return (value - detail::base_offset(From)) * conversion_factor(detail::unitdiff_t<From>{}, detail::unitdiff_t<To>{}) + detail::base_offset(To{});
}

}
}


#include "mex.h"
#include "RigidBodyManipulator.h"
#include "constraint/RigidBodyConstraint.h"
#include "IKoptions.h"
#include "RigidBodyIK.h"
#include <Eigen/Dense>
#include "drakeUtil.h"

using namespace std;
using namespace Eigen;

void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
{
  if(nrhs < 7)
  {
    mexErrMsgIdAndTxt("Drake:inverseKinTrajmex:NotEnoughInputs","Usage inverseKinPointwisemex(model_ptr,t,qdot0_seed,q_seed,q_nom,constraint1,constraint2,...,ikoptions");
  }
  RigidBodyManipulator* model = (RigidBodyManipulator*) getDrakeMexPointer(prhs[0]);
  int nq = model->num_dof;
  int nT = static_cast<int>(mxGetNumberOfElements(prhs[1]));
  double* t = mxGetPr(prhs[1]);
  Map<VectorXd> qdot0_seed(mxGetPr(prhs[2]),nq);
  Map<MatrixXd> q_seed(mxGetPr(prhs[3]),nq,nT);
  Map<MatrixXd> q_nom(mxGetPr(prhs[4]),nq,nT);
  int num_constraints = nrhs-6;
  RigidBodyConstraint** constraint_array = new RigidBodyConstraint*[num_constraints];
  for(int i = 0;i<num_constraints;i++)
  {
    constraint_array[i] = (RigidBodyConstraint*) getDrakeMexPointer(prhs[5+i]);
  }
  IKoptions* ikoptions = (IKoptions*) getDrakeMexPointer(prhs[nrhs-1]);
  plhs[0] = mxCreateDoubleMatrix(nq,nT,mxREAL);
  Map<MatrixXd> q_sol(mxGetPr(plhs[0]),nq,nT);
  plhs[1] = mxCreateDoubleMatrix(nq,nT,mxREAL);
  Map<MatrixXd> qdot_sol(mxGetPr(plhs[1]),nq,nT);
  plhs[2] = mxCreateDoubleMatrix(nq,nT,mxREAL);
  Map<MatrixXd> qddot_sol(mxGetPr(plhs[2]),nq,nT);
  int info = 0;
  vector<string> infeasible_constraint;
  inverseKinTraj(model,nT,t,qdot0_seed,q_seed,q_nom,num_constraints,constraint_array,q_sol,qdot_sol,qddot_sol,info,infeasible_constraint,*ikoptions);
  plhs[3] = mxCreateDoubleScalar((double) info);
  mwSize name_dim[1] = {static_cast<mwSize>(infeasible_constraint.size())};
  plhs[4] = mxCreateCellArray(1,name_dim);
  for(int i = 0;i<infeasible_constraint.size();i++)
  {
    mxArray* name_ptr = mxCreateString(infeasible_constraint[i].c_str());
    mxSetCell(plhs[4],i,name_ptr);
  }
  delete[] constraint_array;
}


//
// GraphTools library
// Copyright 2017-2019 Illumina, Inc.
// All rights reserved.
//
// Author: Roman Petrovski <RPetrovski@illumina.com>
//
// 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.
//

#pragma once

#include <list>
#include <map>
#include <string>

#include <boost/throw_exception.hpp>

#include "graphalign/DagAlignerAffine.hh"
#include "graphalign/GraphAlignment.hh"
#include "graphalign/Operation.hh"
#include "graphalign/dagAligner/BaseMatchingPenaltyMatrix.hh"
#include "graphcore/Graph.hh"
#include "graphcore/Path.hh"

namespace graphtools
{

using PathAndAlignment = std::pair<Path, Alignment>;

template <bool penalizeMove, bool clipFront = true>
class BaseMatchingDagAligner : public graphalign::dagAligner::Aligner<
                                   graphalign::dagAligner::AffineAlignMatrixVectorized<
                                       graphalign::dagAligner::BaseMatchingPenaltyMatrix, penalizeMove>,
                                   clipFront>
{
    typedef graphalign::dagAligner::BaseMatchingPenaltyMatrix PenaltyMatrix;
    typedef graphalign::dagAligner::Score Score;

public:
    BaseMatchingDagAligner(const PenaltyMatrix& penaltyMatrix, Score gapOpen, Score gapExt)
        : graphalign::dagAligner::Aligner<
              graphalign::dagAligner::AffineAlignMatrixVectorized<PenaltyMatrix, penalizeMove>, clipFront>(
              penaltyMatrix, gapOpen, gapExt)
    {
    }

    BaseMatchingDagAligner(graphalign::dagAligner::Score match, Score mismatch, Score gapOpen, Score gapExt)
        : graphalign::dagAligner::Aligner<
              graphalign::dagAligner::AffineAlignMatrixVectorized<PenaltyMatrix, penalizeMove>, clipFront>(
              PenaltyMatrix(match, mismatch), gapOpen, gapExt)
    {
    }
};

/**
 * Performs alignment of query pieces that start or end at the seed in the graph
 */
class PinnedDagAligner
{
    typedef std::pair<int, int> Edge;
    typedef std::vector<Edge> Edges;
    typedef graphalign::dagAligner::Cigar Cigar;
    BaseMatchingDagAligner<true, false> aligner_;

    static void appendOperation(OperationType type, uint32_t length, std::list<Operation>& operations)
    {
        if (operations.empty() || operations.back().type() != type)
        {
            operations.push_back(Operation(type, length));
        }
        else
        {
            operations.back() = Operation(type, operations.back().length() + length);
        }
    }

    template <typename GraphT, typename PathT>
    static void parseGraphCigar(
        const GraphT& graph, const graphalign::dagAligner::Cigar& cigar, PathT& path, std::list<Operation>& operations)
    {
        using namespace graphalign::dagAligner;
        for (const Cigar::Operation& op : cigar)
        {
            if (Cigar::NODE_START == op.code_)
            {
                if (path.lastNodeId() != op.value_
                    || std::size_t(path.endPosition()) == graph.nodeSeq(op.value_).length())
                {
                    path.extendEndToNode(op.value_);
                }
            }
            else if (Cigar::NODE_END == op.code_)
            {
            }
            else
                switch (op.code_)
                {
                case Cigar::MATCH:
                {
                    appendOperation(OperationType::kMatch, op.value_, operations);
                    path.shiftEndAlongNode(op.value_);
                    break;
                }
                case Cigar::MISMATCH:
                {
                    appendOperation(OperationType::kMismatch, op.value_, operations);
                    path.shiftEndAlongNode(op.value_);
                    break;
                }
                case Cigar::INSERT:
                {
                    appendOperation(OperationType::kInsertionToRef, op.value_, operations);
                    break;
                }
                case Cigar::SOFT_CLIP:
                {
                    appendOperation(OperationType::kSoftclip, op.value_, operations);
                    break;
                }
                case Cigar::DELETE:
                {
                    appendOperation(OperationType::kDeletionFromRef, op.value_, operations);
                    path.shiftEndAlongNode(op.value_);
                    break;
                }
                default:
                {
                    throw std::logic_error(std::string("Unexpected graph cigar operation:") + std::to_string(op.code_));
                }
                }
        }
    }

    typedef int MappedId;

    void unmapNodeIds(const std::map<MappedId, NodeId>& originalIds, graphalign::dagAligner::Cigar& cigar)
    {
        using namespace graphalign::dagAligner;
        for (Cigar::Operation& op : cigar)
        {
            if (Cigar::NODE_START == op.code_ || Cigar::NODE_END == op.code_)
            {
                op.value_ = originalIds.at(op.value_);
            }
        }
    }

    /**
     * \brief Tests if the cigar first node is a repeat expansion and corrects
     *        cigar to ensure that fraction of the first expansion is interpreted correctly
     */
    template <typename PathT>
    void fixFirstNodeExpansion(
        const std::vector<MappedId>& nodeIds, const std::map<MappedId, NodeId>& originalIds, const PathT& seedPath,
        graphalign::dagAligner::Cigar& cigar)
    {
        using namespace graphalign::dagAligner;

        if (seedPath.lastNodeId() != originalIds.at(cigar.firstNode()) || MappedId(cigar.firstNode()) != nodeIds[0])
        {
            const int skipLen
                = seedPath.graphRawPtr()->nodeSeq(seedPath.lastNodeId()).length() - seedPath.endPosition();
            if (0 > skipLen)
            {
                throw std::logic_error("invalid distance to skip");
            }
            if (skipLen)
            {
                const Cigar::Operation skipFirstNode[]
                    = { Cigar::Operation(
                            Cigar::NODE_START, nodeIds[0]), // endPosition is on the base that belongs to the path...
                        Cigar::Operation(Cigar::DELETE, skipLen), Cigar::Operation(Cigar::NODE_END, nodeIds[0]) };

                cigar.insert(
                    cigar.begin(), skipFirstNode, skipFirstNode + sizeof(skipFirstNode) / sizeof(skipFirstNode[0]));
            }
        }
    }

public:
    explicit PinnedDagAligner(
        const int32_t matchScore, const int32_t mismatchScore, const int32_t gapOpenScore, const int32_t gapExtendScore)
        : aligner_(matchScore, mismatchScore, gapOpenScore, gapExtendScore)
    {
    }

    std::list<PathAndAlignment>
    prefixAlign(const Path& seedPath, const std::string& queryPiece, size_t extensionLen, int& score)
    {
        using namespace graphalign::dagAligner;
        std::vector<MappedId> nodeIds;
        Edges edges;
        std::string target;

        // when repeat expansions are unrolled each copy gets a unique id, so, all
        // ids have to be remapped
        std::map<MappedId, NodeId> originalIds;

        bfsDiscoverEdges(
            *seedPath.graphRawPtr(), seedPath.nodeIds().back(), seedPath.endPosition(), extensionLen, nodeIds, edges,
            target, originalIds);
        edges.push_back(Edge(target.length(), target.length()));

        std::list<PathAndAlignment> ret;
        if (!target.empty())
        {
            const EdgeMap alignerEdges(edges, nodeIds);

            aligner_.align(queryPiece.begin(), queryPiece.end(), target.begin(), target.end(), alignerEdges);

            std::vector<Cigar> cigars;
            Score secondBestScore = 0;
            Score bestScore = aligner_.backtrackAllPaths<false>(alignerEdges, cigars, secondBestScore);

            score = bestScore;
            for (Cigar& cigar : cigars)
            {
                fixFirstNodeExpansion(nodeIds, originalIds, seedPath, cigar);

                unmapNodeIds(originalIds, cigar);

                Path path = seedPath;
                std::list<Operation> operations;
                parseGraphCigar(*seedPath.graphRawPtr(), cigar, path, operations);

                ret.push_back(PathAndAlignment(path, Alignment(seedPath.seq().length(), operations)));
            }
        }

        return ret;
    }

    std::list<PathAndAlignment>
    suffixAlign(const Path& seedPath, std::string queryPiece, size_t extensionLen, int& score)
    {
        using namespace graphalign::dagAligner;
        std::vector<MappedId> nodeIds;
        Edges edges;
        std::string target;

        // when repeat expansions are unrolled each copy gets a unique id, so, all
        // ids have to be remapped
        std::map<MappedId, NodeId> originalIds;

        ReverseGraph rg(*seedPath.graphRawPtr());
        bfsDiscoverEdges(
            rg, seedPath.nodeIds().front(),
            // endPosition is on the base that belongs to the path...
            ConstReversePath(seedPath).endPosition(), extensionLen, nodeIds, edges, target, originalIds);
        edges.push_back(Edge(target.length(), target.length()));

        std::list<PathAndAlignment> ret;
        if (!target.empty())
        {
            const EdgeMap alignerEdges(edges, nodeIds);

            std::reverse(queryPiece.begin(), queryPiece.end());
            aligner_.align(queryPiece.begin(), queryPiece.end(), target.begin(), target.end(), alignerEdges);

            std::vector<Cigar> cigars;
            Score secondBestScore = 0;
            Score bestScore = aligner_.backtrackAllPaths<false>(alignerEdges, cigars, secondBestScore);

            score = bestScore;
            for (Cigar& cigar : cigars)
            {
                fixFirstNodeExpansion(nodeIds, originalIds, ConstReversePath(seedPath), cigar);

                unmapNodeIds(originalIds, cigar);

                Path path = seedPath;
                ReversePath rp(path);
                std::list<Operation> operations;
                parseGraphCigar(rg, cigar, rp, operations);
                operations.reverse();

                // reversed alignments always start at the beginning of the path because
                // the seed path gets start-extended to incorporate them
                ret.push_back(PathAndAlignment(path, Alignment(0, operations)));
            }
        }

        return ret;
    }

private:
    template <typename GraphT>
    static std::map<NodeId, int> extractSubgraph(
        const GraphT& graph, const NodeId startNodeId, const std::size_t startNodeOffset, const std::size_t seqLen)
    {
        std::map<NodeId, int> nodeStartSeqOffset;

        if (graph.nodeSeq(startNodeId).length() == startNodeOffset)
        {
            // flag empty start node in a special way
            nodeStartSeqOffset[startNodeId] = -1;
        }
        else
        {
            // first node start is at the start of the sequence
            nodeStartSeqOffset[startNodeId] = 0;
        }

        // nodes to be visited by bfs
        std::deque<NodeId> shouldVisit(1, startNodeId);

        // extract longest subgraph of nodes such that each node begins within
        // seqLen from the start of the start node
        while (!shouldVisit.empty())
        {
            const NodeId currentNodeId = shouldVisit.front();
            shouldVisit.pop_front();

            const std::string& currentNodeSeq = graph.nodeSeq(currentNodeId);
            const int currentNodeSeqOffset = nodeStartSeqOffset[currentNodeId];

            // avoid dealing with individual node startNodeOffset (only start node has it)
            // by pretending the sequence starts at the start node start
            if (seqLen + startNodeOffset - std::max(0, currentNodeSeqOffset) > currentNodeSeq.length())
            {
                const int successorSeqOffset
                    = -1 == currentNodeSeqOffset ? 0 : currentNodeSeqOffset + currentNodeSeq.length();
                // sequence does not terminate at this node, enqueue successors
                const std::set<NodeId>& successors = graph.successors(currentNodeId);
                for (const NodeId successorId : successors)
                {
                    const auto seenSuccessor = nodeStartSeqOffset.find(successorId);
                    if (nodeStartSeqOffset.end() == seenSuccessor || seenSuccessor->second > successorSeqOffset)
                    {
                        // some successors will end up listed in shouldVisit more than once at a time
                        shouldVisit.push_back(successorId);
                        nodeStartSeqOffset[successorId] = successorSeqOffset;
                    }
                }
            }
        }

        return nodeStartSeqOffset;
    }

    typedef std::pair<MappedId, MappedId> IdEdge;
    typedef std::vector<IdEdge> IdEdges;

    /**
     * \brief expands repeats up to remainder of sequnce length
     * \return pairs of mapped node ids indicating an edge between them.
     * \postcondition result array is ordered by successor id then by predecessor id
     */
    template <typename GraphT>
    static IdEdges unrollRepeats(
        const GraphT& graph, const std::size_t seqLen, const std::map<NodeId, int>& nodeStartSeqOffset,
        std::map<MappedId, NodeId>& originalIds, std::multimap<NodeId, MappedId>& mappedIds)
    {
        IdEdges idEdges;
        for (const auto& nodeIdOffset : nodeStartSeqOffset)
        {
            if (int(seqLen) <= nodeIdOffset.second)
            {
                throw std::logic_error("node should not be in the subgraph");
            }

            const std::set<NodeId>& successors = graph.successors(nodeIdOffset.first);
            if (successors.end() != successors.find(nodeIdOffset.first))
            {
                const std::size_t nodeSeqLen = graph.nodeSeq(nodeIdOffset.first).length();
                // don't forget empty repeat start node has special sequence offset -1
                for (std::size_t lenLeft = seqLen - std::max(0, nodeIdOffset.second); lenLeft;)
                {
                    // chain unrolled repeat nodes together
                    IdEdge edge(mappedIds.size(), mappedIds.size() + 1);
                    idEdges.push_back(edge);
                    mappedIds.emplace(nodeIdOffset.first, originalIds.size());
                    originalIds.emplace(originalIds.size(), nodeIdOffset.first);
                    lenLeft -= std::min(lenLeft, nodeSeqLen);
                }

                // since edges point forward, above loop always produces one more edge than we need
                idEdges.pop_back();
            }
            else
            {
                mappedIds.emplace(nodeIdOffset.first, originalIds.size());
                originalIds.emplace(originalIds.size(), nodeIdOffset.first);
            }
        }

        linkPredecessors(graph, originalIds, mappedIds, idEdges);

        // group edges by successor node
        std::sort(idEdges.begin(), idEdges.end(), [](const IdEdge& left, const IdEdge& right) {
            return left.second < right.second || (left.second == right.second && left.first < right.first);
        });

        return idEdges;
    }

    static void dfsExtractOrderedNodeIds(
        const MappedId currentId, const IdEdges& idEdges, const std::vector<std::size_t>& idEdgesIndex,
        std::vector<char>& seenNodes, std::vector<MappedId>& nodeIds)
    {
        if (!seenNodes.at(currentId))
        {
            seenNodes[currentId] = true;
            for (std::size_t predOffset = idEdgesIndex.at(currentId); idEdgesIndex.at(currentId + 1) != predOffset;
                 ++predOffset)
            {
                if (idEdges.at(predOffset).second != currentId)
                {
                    throw std::logic_error(
                        "dfsExtractOrderedNodeIds: Invalid edge for node " + std::to_string(currentId));
                }

                dfsExtractOrderedNodeIds(idEdges[predOffset].first, idEdges, idEdgesIndex, seenNodes, nodeIds);
            }
            nodeIds.push_back(currentId);
        }
    }
    /**
     * \brief dfs in order to produce the topological ordering with start node on top
     */
    static std::vector<MappedId>
    extractOrderedNodeIds(const IdEdges& idEdges, const std::vector<std::size_t>& idEdgesIndex)
    {
        std::vector<MappedId> nodeIds;
        std::vector<char> seenNodes(idEdgesIndex.size(), false);
        for (std::size_t mappedId = 0; idEdgesIndex.size() - 1 != mappedId; ++mappedId)
        {
            dfsExtractOrderedNodeIds(mappedId, idEdges, idEdgesIndex, seenNodes, nodeIds);
        }

        return nodeIds;
    }

    /**
     * \brief self-repeat edges already in idEdges, add all mapped predecessor edges to each first expansion
     *        and non-repeat nodes
     */
    template <typename GraphT>
    static void linkPredecessors(
        const GraphT& graph, const std::map<MappedId, NodeId>& originalIds,
        const std::multimap<NodeId, MappedId>& mappedIds, IdEdges& idEdges)
    {
        for (MappedId mappedId = 0; MappedId(mappedIds.size()) != mappedId;)
        {
            const NodeId originalId = originalIds.at(mappedId);

            bool selfRepeat = false;
            for (const NodeId& predecessorId : graph.predecessors(originalId))
            {
                if (predecessorId != originalId)
                {
                    auto mappedPredIds = mappedIds.equal_range(predecessorId);

                    // other nodes, insert edges for each predecessor instance
                    // empty ranges indicate predecessors that are not part of subgraph
                    while (mappedPredIds.second != mappedPredIds.first)
                    {
                        IdEdge edge(mappedPredIds.first->second, mappedId);
                        idEdges.push_back(edge);
                        ++mappedPredIds.first;
                    }
                }
                else
                {
                    selfRepeat = true;
                }
            }

            if (selfRepeat)
            {
                // skip all instances of self repeat so that only first node gets edges
                // from predecessors
                auto mappedIdsRange = mappedIds.equal_range(originalId);
                mappedId += std::distance(mappedIdsRange.first, mappedIdsRange.second);
            }
            else
            {
                ++mappedId;
            }
        }
    }

    static std::vector<std::size_t> indexEdges(const IdEdges& idEdges, const std::map<MappedId, NodeId>& originalIds)
    {
        std::vector<std::size_t> index;
        index.reserve(idEdges.size() + 1);
        index.push_back(0);
        if (!idEdges.size())
        {
            // no edges. Single-node graph, just close the index
            index.push_back(0);
        }
        else
        {
            MappedId lastId = -1;
            for (const IdEdge& edge : idEdges)
            {
                while (lastId != edge.second)
                {
                    index.push_back(index.back());
                    ++lastId;
                }
                ++index.back();
            }
            // add nodes without predecessors to index
            while (lastId != MappedId(originalIds.rbegin()->first))
            {
                index.push_back(index.back());
                ++lastId;
            }
        }

        return index;
    }

    template <typename GraphT>
    static std::string buildTargetSequence(
        const GraphT& graph, const std::size_t startNodeOffset, const std::vector<MappedId>& nodeIds,
        const std::map<MappedId, NodeId>& originalIds, const std::map<NodeId, int>& nodeStartSeqOffset,
        const std::vector<std::size_t>& idEdgesIndex, const IdEdges& idEdges, std::vector<Edge>& edges)
    {
        std::string target;
        std::vector<int> mappedIdEndOffset(nodeIds.size(), 0);
        // when first node is a repeat expansion fully consumed by seed, just pretend that query start
        // at the beginning of the node rather than after the end...
        std::size_t startOffset = startNodeOffset;
        for (const MappedId mappedId : nodeIds)
        {
            const NodeId originalId = originalIds.at(mappedId);
            const std::string& nodeSeq = graph.nodeSeq(originalId);
            if (!startOffset || -1 != nodeStartSeqOffset.at(originalId))
            {
                const std::size_t nodePartLen = nodeSeq.length() - startOffset;
                if (!nodePartLen)
                {
                    throw std::logic_error("Empty node in expanded subgraph and it is not the first one");
                }

                for (std::size_t predOffset = idEdgesIndex[mappedId]; idEdgesIndex[mappedId + 1] != predOffset;
                     ++predOffset)
                {
                    if (idEdges[predOffset].second != mappedId)
                    {
                        throw std::logic_error("bfsDiscoverEdges: Invalid edge");
                    }
                    Edge edge(mappedIdEndOffset.at(idEdges[predOffset].first), target.length());
                    edges.push_back(edge);
                }
                target += nodeSeq.substr(startOffset, nodePartLen);
            }
            mappedIdEndOffset[mappedId] = target.length() - 1;
            startOffset = 0;
        }

        return target;
    }

    /**
     * \param seqLen    extensionLen + offset in the first node. This simplifies
     * the process
     *                  by assuming that sequence starts at the node start
     */
    template <typename GraphT>
    static void bfsDiscoverEdges(
        const GraphT& graph, const NodeId startNodeId, const std::size_t startNodeOffset, const std::size_t seqLen,
        std::vector<MappedId>& nodeIds, std::vector<Edge>& edges, std::string& target,
        std::map<MappedId, NodeId>& originalIds)
    {
        // length of shortest path to the first node character
        const std::map<NodeId, int> nodeStartSeqOffset = extractSubgraph(graph, startNodeId, startNodeOffset, seqLen);

        // Repeat expansions need to be unrolled, so we create unique id for each unrolled instance and map
        // them to original ids
        std::multimap<NodeId, MappedId> mappedIds;
        const IdEdges idEdges
            = unrollRepeats(graph, startNodeOffset + seqLen, nodeStartSeqOffset, originalIds, mappedIds);

        // index the edge array so that for each mappedId there is a pair of entries index[mappedId],
        // index[mappedId+1] which contains offsets of all predecessor edges in the idEdges;
        const std::vector<std::size_t> idEdgesIndex = indexEdges(idEdges, originalIds);

        nodeIds = extractOrderedNodeIds(idEdges, idEdgesIndex);
        if (nodeIds.size() != mappedIds.size())
        {
            throw std::logic_error(
                "Invalid number of nodeIds entries " + std::to_string(nodeIds.size()) + " expected "
                + std::to_string(mappedIds.size()));
        }

        if (originalIds.at(nodeIds.at(0)) != startNodeId)
        {
            // nodeIds must be topologically sorted. Since we're discovering our subgraph from the
            // start node, its first expansion must sort to the top.
            throw std::logic_error("First expansion of start node must be the first node");
        }

        // extract target sequence in the proper order
        target = buildTargetSequence(
            graph, startNodeOffset, nodeIds, originalIds, nodeStartSeqOffset, idEdgesIndex, idEdges, edges);

        if (-1 == nodeStartSeqOffset.at(startNodeId))
        {
            // if the first node is an empty repeat expansion (consumed by seed), remove it as it has no
            // corresponding sequence in the target.
            nodeIds.erase(nodeIds.begin());
        }
    }
};
}


// Copyright (c) Dewetron 2017
#include "otfft.h"

#include <boost/test/unit_test.hpp>

#include <chrono>
#include <cmath>
#include <cstdint>
#include <future>
#include <thread>
#include <unordered_map>

namespace
{
    typedef struct MemberCapsule
    {
        MemberCapsule()
            : active(false)
            , fft()
        {
            BOOST_TEST_MESSAGE("MemberCapsule::MemberCapsule()");
        }
        MemberCapsule(MemberCapsule&& other)
            : active(std::move(other.active))
            , fft(std::move(other.fft))
        {
            BOOST_TEST_MESSAGE("MemberCapsule::MemberCapsule(MemberCapsule&& other)");
        }
        MemberCapsule& operator=(MemberCapsule&& other)
        {
            BOOST_TEST_MESSAGE("MemberCapsule::operator=(MemberCapsule&& other)");
            if (this != &other)
            {
                active = std::move(other.active);
                fft = std::move(other.fft);
            }
            return *this;
        }
        MemberCapsule(const MemberCapsule&)
        {
            BOOST_ERROR("MemberCapsule::MemberCapsule(const MemberCapsule&)");
        }
        MemberCapsule& operator=(const MemberCapsule&)
        {
            BOOST_ERROR("MemberCapsule::operator=(const MemberCapsule&)");
            return *this;
        }

        bool active;
        struct FFTMemberStruct
        {
            FFTMemberStruct()
                : rfft(nullptr)
                , cfft(nullptr)
                , dct(nullptr)
                , bluestein(nullptr)
            {
                BOOST_TEST_MESSAGE("FFTMemberStruct::FFTMemberStruct()");
            }
            FFTMemberStruct(FFTMemberStruct&& other)
                : rfft(std::move(other.rfft))
                , cfft(std::move(other.cfft))
                , dct(std::move(other.dct))
                , bluestein(std::move(other.bluestein))
            {
                BOOST_TEST_MESSAGE("FFTMemberStruct::FFTMemberStruct(FFTMemberStruct&& other)");
            }
            FFTMemberStruct& operator=(FFTMemberStruct&& other)
            {
                BOOST_TEST_MESSAGE("FFTMemberStruct::operator=(FFTMemberStruct&& other)");
                if (this != &other)
                {
                    rfft = std::move(other.rfft);
                    cfft = std::move(other.cfft);
                    dct = std::move(other.dct);
                    bluestein = std::move(other.bluestein);
                }
                return *this;
            }
            FFTMemberStruct(const FFTMemberStruct&)
            {
                BOOST_ERROR("FFTMemberStruct::FFTMemberStruct(const FFTMemberStruct&)");
            }
            FFTMemberStruct& operator=(const FFTMemberStruct&)
            {
                BOOST_ERROR("FFTMemberStruct::operator=(const FFTMemberStruct&)");
                return *this;
            }

            OTFFT::RealFFTPtr rfft;
            OTFFT::ComplexFFTPtr cfft;
            OTFFT::RealDCTPtr dct;
            OTFFT::ComplexFFTPtr bluestein;
        } fft;
    } MemberCapsule;

    class FFTClass;
    void ThreadHelper(FFTClass* fft_class);

    class FFTClass
    {
    public:
        explicit FFTClass(const std::size_t fft_size)
            : m_capsule()
            , m_thread()
            , m_fft_size(fft_size)
        {
            BOOST_TEST_MESSAGE("FFTClass::FFTClass()");
        }
        ~FFTClass()
        {
            BOOST_TEST_MESSAGE("FFTClass::~FFTClass()");
            m_capsule.active = false;
            if (m_thread.joinable())
            {
                m_thread.join();
            }
        }
        FFTClass(FFTClass&& other)
            : m_capsule(std::move(other.m_capsule))
            , m_thread(std::move(other.m_thread))
        {
            BOOST_TEST_MESSAGE("FFTClass::FFTClass(FFTClass&& other)");
        }
        FFTClass& operator=(FFTClass&& other)
        {
            BOOST_TEST_MESSAGE("FFTClass::operator=(FFTClass&& other)");
            if (this != &other)
            {
                m_capsule = std::move(other.m_capsule);
                m_thread = std::move(other.m_thread);
            }
            return *this;
        }

        void start()
        {
            BOOST_TEST_MESSAGE("FFTClass::start()");
            m_capsule.active = true;

            m_capsule.fft.cfft = OTFFT::Factory::createComplexFFT(static_cast<int>(m_fft_size));
            m_capsule.fft.bluestein = OTFFT::Factory::createBluesteinFFT(static_cast<int>(m_fft_size));

            if ((m_fft_size & 1) == 0)
            {
                m_capsule.fft.rfft = OTFFT::Factory::createRealFFT(static_cast<int>(m_fft_size));
                m_capsule.fft.dct = OTFFT::Factory::createDCT(static_cast<int>(m_fft_size));
            }

            m_thread = std::thread(ThreadHelper, this);
        }
        void stop()
        {
            BOOST_TEST_MESSAGE("FFTClass::stop()");
            m_capsule.active = false;
            if (m_thread.joinable())
            {
                m_thread.join();
            }
        }

        void runThread()
        {
            BOOST_TEST_MESSAGE("FFTClass::runThread - Enter");
            while (m_capsule.active)
            {
                std::this_thread::sleep_for(std::chrono::milliseconds(1));

                m_capsule.fft.cfft->setup(static_cast<int>(m_fft_size));
                m_capsule.fft.bluestein->setup(static_cast<int>(m_fft_size));

                if ((m_fft_size & 1) == 0)
                {
                    m_capsule.fft.rfft->setup(static_cast<int>(m_fft_size));
                    m_capsule.fft.dct->setup(static_cast<int>(m_fft_size));
                }
            }
            BOOST_TEST_MESSAGE("FFTClass::runThread - Exit");
        }

    private:
        FFTClass(const FFTClass&)
        {
            BOOST_ERROR("FFTClass::FFTClass(const FFTClass&)");
        }
        FFTClass& operator=(const FFTClass&)
        {
            BOOST_ERROR("FFTClass::operator=(const FFTClass&)");
            return *this;
        }

    private:
        MemberCapsule m_capsule;
        std::thread m_thread;
        std::size_t m_fft_size;
    };

    typedef std::shared_ptr<FFTClass> FFTClassPtr;

    void ThreadHelper(FFTClass* fft_class)
    {
        fft_class->runThread();
    }
}

BOOST_AUTO_TEST_SUITE(otfft_basic_test)

BOOST_AUTO_TEST_CASE(TestFactory)
{
    BOOST_TEST_MESSAGE("Checking OTFFT factory");

    const std::size_t N = static_cast<std::size_t>(std::pow(2, 8));
    std::unordered_map<std::uint64_t, FFTClassPtr> fft_classes;

    for (std::size_t n = 8; n < N; n += static_cast<std::size_t>(std::max(1, static_cast<int>(std::log2(n)))))
    {
        {
            BOOST_TEST_MESSAGE("-> Create FFT Class with FFT order " << n);
            auto fft_class = std::make_shared<FFTClass>(n);
            fft_classes[n] = fft_class;
        }
        {
            BOOST_TEST_MESSAGE("-> Start Thread");
            auto fft_class = fft_classes[n];
            std::async(std::launch::async, [fft_class]{ fft_class->start(); }).wait();
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
        }
        {
            BOOST_TEST_MESSAGE("-> Stop Thread");
            auto fft_class = fft_classes[n];
            std::async(std::launch::async, [fft_class]{ fft_class->stop(); }).wait();
        }
    }

    BOOST_TEST_MESSAGE("-> Release Resources");
    fft_classes.clear();

    BOOST_CHECK(true);
}

BOOST_AUTO_TEST_SUITE_END()


#include "stddefx.h"

#ifndef INCLUDED_CALC_LOOKUPTABLE
#include "calc_lookuptable.h"
#define INCLUDED_CALC_LOOKUPTABLE
#endif

#ifndef INCLUDED_TABLE
#include "table.h"    // LOOK_UP_TABLE
#define INCLUDED_TABLE
#endif
#ifndef INCLUDED_COM_INTERVALTYPES
#include "com_intervaltypes.h"
#define INCLUDED_COM_INTERVALTYPES
#endif
#ifndef INCLUDED_COM_CLONE
#include "com_clone.h"
#define INCLUDED_COM_CLONE
#endif

#ifndef INCLUDED_ALGORITHM
#include <algorithm>
#define INCLUDED_ALGORITHM
#endif

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

#ifndef INCLUDED_CALC_LIBERROR
#include "calc_liberror.h"
#define INCLUDED_CALC_LIBERROR
#endif

#ifndef INCLUDED_CALC_MAP2CSF
#include "calc_map2csf.h"
#define INCLUDED_CALC_MAP2CSF
#endif

#ifndef INCLUDED_CALC_QUOTE
#include "calc_quote.h"
#define INCLUDED_CALC_QUOTE
#endif

LOOK_UP_TABLE *calc::LookupTable::createOldStyle(
  const std::string &fileName)
{
  LOOK_UP_TABLE *t(0);
  FILE  *f = fopen(fileName.c_str(), "r");
  if (!f)
    libError("Can't open lookup table "+quote(fileName));

  try {
   std::vector<CSF_VS> keyTypes(d_keyVs.size());
   for(size_t i=0; i < d_keyVs.size(); i++)
     keyTypes[i] = vs2CsfVs(biggestVs(d_keyVs[i]));

   t = ReadLookupTable(f, &(keyTypes[0]), keyTypes.size(), vs2CsfVs(d_vs));
   if (!t) {
     // pcrcalc/test10a
     libError("while parsing lookuptable "+quote(fileName));
   }
  } catch ( ... ) {
     fclose(f);
     throw;
  }

  fclose(f);
  return t;
}

calc::LookupTable::LookupTable(
    VS outType):
    d_vs(outType)
{
}

/*! create subset with keys removed that match \a filterKeys
 */
calc::LookupTable::LookupTable(
    const LookupTable& t,
    const std::vector<bool>&   remove,
    const std::vector<double>& filterKeys):
  d_vs(t.d_vs)
{
  std::vector<size_t> keep;
  for(size_t k=0; k < t.d_keyVs.size(); ++k)
   if (!remove[k]) {
    d_keyVs.push_back(t.d_keyVs[k]);
    keep.push_back(k);
  }
  for(size_t i=0; i < t.d_records.size(); ++i)
    if (t.d_records[i].match(remove,filterKeys))
      d_records.push_back(LookupRecord(t.d_records[i],keep));
}

//! parse records from ASCII file
/*!
 * \throws com::Exception in case of error
 */
void calc::LookupTable::setRecords(
  const std::string &fileName,
  const std::vector<VS>& inKeys)
{
  d_keyVs = inKeys;
  LOOK_UP_TABLE *table(createOldStyle(fileName));

  try {
    d_records.reserve(table->nrRecords);
    for(size_t r=0; r < table->nrRecords; r++)
     d_records.push_back(LookupRecord(table->records[r],table->nrKeys));
  } catch(...) {
    FreeLookupTable(table);
    throw;
  }
  FreeLookupTable(table);
}

//! set records
void calc::LookupTable::setRecords(
    const Records& records,
    const std::vector<VS>& inKeys)
{
    d_records=records;
    d_keyVs=inKeys;
}

//! dtor
calc::LookupTable::~LookupTable()
{
}

calc::LookupRecord::LookupRecord(const Key& key, double result):
  d_result(result)
{
  for(size_t k=0; k < key.size(); k++) {
    d_key.push_back(key[k]->createClone());
  }
}

//! copy a selection of keys
calc::LookupRecord::LookupRecord(
    const LookupRecord& l,
    const std::vector<size_t>& select):
  d_result(l.d_result)
{
  for(size_t k=0; k < select.size(); k++) {
    d_key.push_back(l.d_key[select[k]]->createClone());
  }
}

calc::LookupRecord::LookupRecord(const LOOK_UP_KEY *keys,size_t nrKeys)
{
  d_key.resize(nrKeys);
  for(size_t k=0; k < nrKeys; k++) {
     const LOOK_UP_KEY *l=keys+k;
     switch (l->t) {
       case TEST_ONE    : d_key[k]= new com::EqualTo<>(l->l);
                          break;
       case TEST_INF_INF: d_key[k]= new com::AnythingInterval<>();       // infinity
                          break;
       case TEST_GE_INF : d_key[k]= new com::GreaterThanEqualTo<>(l->l);  // [l  ,inf>
                          break;
       case TEST_GT_INF : d_key[k]= new com::GreaterThan<>(l->l);         // <l  ,inf>
                          break;
       case TEST_INF_LE : d_key[k]= new com::LessThanEqualTo<>(l->h);     // <inf,h]
                          break;
       case TEST_GE_LE  : d_key[k]= new com::BetweenLimits<>(
                              com::GreaterThanEqualTo<>(l->l),
                              com::LessThanEqualTo<>(l->h));              // [l  ,h]
                          break;
       case TEST_GT_LE  : d_key[k]= new com::BetweenLimits<>(
                              com::GreaterThan<>(l->l),
                              com::LessThanEqualTo<>(l->h));              // <l  ,h]
                          break;
       case TEST_INF_LT : d_key[k]= new com::LessThan<>(l->h);            // <inf,h>
                          break;
       case TEST_GE_LT  : d_key[k]= new com::BetweenLimits<>(
                              com::GreaterThanEqualTo<>(l->l),
                              com::LessThan<>(l->h));                     // [l  ,h>
                          break;
       case TEST_GT_LT  : d_key[k]= new com::BetweenLimits<>(
                              com::GreaterThan<>(l->l),
                              com::LessThan<>(l->h));                     // <l  ,h>
                          break;
     }
   }
   POSTCOND(keys[nrKeys].t == TEST_ONE);
   d_result=keys[nrKeys].l;
}

void calc::LookupRecord::cloneKey(const Key& key)
{
  com::copyClone(key,d_key);
}

calc::LookupRecord&
calc::LookupRecord::operator=(const LookupRecord& r)
{
  if (this != &r) {
   d_result=r.d_result;
   cloneKey(r.d_key);
  }
  return *this;
}

calc::LookupRecord::LookupRecord(const LookupRecord& r):
 d_result(r.d_result)
{
   cloneKey(r.d_key);
}

//! delete all records of \a key and clear the vector
void calc::LookupRecord::deleteKey(Key& key)
{
  com::clearClone(key);
}

calc::LookupRecord::~LookupRecord()
{
  deleteKey(d_key);
}

//! match only parts of key where \a mustMatch is true
/*! if mustMatch is false then that part of the key always match
 */
bool calc::LookupRecord::match(
              const std::vector<bool>&   mustMatch,
              const std::vector<double>& key) const
{
  DEVELOP_PRECOND(d_key.size() == key.size());
  for(size_t i=0; i < d_key.size(); i++)
    if (mustMatch[i] && !d_key[i]->valid(key[i]))
      return false;
  return true;
}

//! predicate if LookupRecord matches
bool calc::LookupRecord::match(const std::vector<double>& key) const
{
  DEVELOP_PRECOND(d_key.size() == key.size());
  for(size_t i=0; i < d_key.size(); i++)
    if (!d_key[i]->valid(key[i]))
      return false;
  return true;
}

//! compare record in strcmp fashion
/*!
 * \returns 0 if \a key equals this, -1 (< 0) if this is less than \a key,
 *                                   1 (> 0) if this is greater than \a key
 */
int calc::LookupRecord::compare(const std::vector<double>& key) const
{
  DEVELOP_PRECOND(d_key.size() == key.size());
  for(size_t i=0; i < d_key.size(); i++) {
    if (!d_key[i]->valid(key[i])) {
        if (d_key[i]->operator<(key[i]))
          return -1;
        if (d_key[i]->operator>(key[i]))
          return  1;
    }
  }
  return 0;
}

//! try to find a key and return result
/*!
 * \param result set to result, untouched if return value is false
 * \param key    the keys to search for
 */
bool calc::LookupTable::find(double& result, const std::vector<double>& key) const
{
  PRECOND(key.size() == nrKeys());
  Records::const_iterator p = std::find_if(d_records.begin(),d_records.end(),
                   boost::bind(&calc::LookupRecord::match,_1,boost::ref(key)));
  if (p != d_records.end()) {
    result = p->result();
    return true;
  }
  return false;
}

const calc::LookupTable::Records& calc::LookupTable::records() const
{
  return d_records;
}

//! VS of value (last) column
VS calc::LookupTable::returnVs() const
{
  return d_vs;
}

//! nr of keys columns in table
size_t calc::LookupTable::nrKeys() const
{
  return d_keyVs.size();
}


#define BOOST_TEST_MODULE "Test GroupableTimeSeriesSet class"

#include <cstdio>
#include <vector>
#include <boost/test/unit_test.hpp>
#include "IO.hpp"
#include "GroupableTimeSeriesSet.hpp"
#include "Exception.hpp"
#include "distance/Distance.hpp"
#include "group/Group.hpp"

using namespace genex;

struct MockDataset
{
  std::string test_10_20_space = "datasets/test/test_10_20_space.txt";
  std::string test_3_10_space = "datasets/test/test_3_10_space.txt";
} data;

BOOST_AUTO_TEST_CASE( groupable_time_series_set_load_space )
{
  GroupableTimeSeriesSet tsSet;
  tsSet.loadData(data.test_10_20_space, 20, 0, " ");

  // BOOST_CHECK_EQUAL( tsSet.getItemLength(), 20 );
  BOOST_CHECK_EQUAL( tsSet.getItemCount(), 10 );
  BOOST_CHECK( tsSet.getFilePath() == data.test_10_20_space );
}

BOOST_AUTO_TEST_CASE( groupable_time_series_grouping_and_reset )
{
  GroupableTimeSeriesSet tsSet;
  tsSet.loadData(data.test_3_10_space, 0, 0, " ");
  int groupCnt = tsSet.groupAllLengths("euclidean", 0.5, 1, false);
  BOOST_CHECK( groupCnt > 2 );
  BOOST_CHECK_EQUAL( tsSet.getTotalNumberOfGroups(), groupCnt );
}

BOOST_AUTO_TEST_CASE( groupable_time_series_not_grouped_exception )
{
  GroupableTimeSeriesSet tsSet;
  tsSet.loadData(data.test_3_10_space, 0, 0, " ");

  // dataset not grouped:
  BOOST_CHECK_THROW( tsSet.getBestMatch(tsSet.getTimeSeries(0)), GenexException );
}

BOOST_AUTO_TEST_CASE( basic_get_best_match )
{
  GroupableTimeSeriesSet tsSet;
  tsSet.loadData(data.test_3_10_space, 0, 0, " ");
  tsSet.normalize();
  tsSet.groupAllLengths("euclidean", 0.5, 1, false);
  candidate_time_series_t best = tsSet.getBestMatch(tsSet.getTimeSeries(0));
  BOOST_TEST( best.dist == 0.0 );
}

BOOST_AUTO_TEST_CASE( groupable_time_series_save_load )
{
  GroupableTimeSeriesSet tsSet;
  std::string fname = "groupable_time_series_save_load.z";
  tsSet.loadData(data.test_3_10_space, 0, 0, " ");
  tsSet.normalize();
  tsSet.groupAllLengths("euclidean", 0.1, 1, false);
  candidate_time_series_t best = tsSet.getBestMatch(tsSet.getTimeSeries(0));

  saveToFile(tsSet, fname);

  GroupableTimeSeriesSet tsSet2;
  tsSet2.loadData(data.test_3_10_space, 0, 0, " ");
  loadFromFile(tsSet2, fname);
  tsSet2.normalize();
  candidate_time_series_t best2 = tsSet2.getBestMatch(tsSet.getTimeSeries(0));

  bool ok = best == best2;
  BOOST_TEST( ok );
  BOOST_CHECK_EQUAL( tsSet.getDistanceName(), tsSet2.getDistanceName() );
  BOOST_CHECK_EQUAL( tsSet.getThreshold(), tsSet2.getThreshold() );
  BOOST_CHECK_EQUAL( tsSet.getTotalNumberOfGroups(), tsSet2.getTotalNumberOfGroups() );
  remove(fname.c_str());
}

BOOST_AUTO_TEST_CASE( groupable_time_series_save_load_incompatible )
{
 GroupableTimeSeriesSet tsSet;
  std::string fname = "groupable_time_series_save_load_incompatible.z";
  tsSet.loadData(data.test_3_10_space, 0, 0, " ");
  tsSet.normalize();
  tsSet.groupAllLengths("euclidean", 0.1, 1, false);

  saveToFile(tsSet, fname);

  GroupableTimeSeriesSet tsSet2;
  tsSet2.loadData(data.test_10_20_space, 0, 0, " ");
  BOOST_CHECK_THROW( loadFromFile(tsSet2, fname), GenexException );
  remove(fname.c_str());
}


#ifndef BOOST_PREPROCESSOR_LOGICAL_NOR_HPP
#define BOOST_PREPROCESSOR_LOGICAL_NOR_HPP

/* Copyright (C) 2001
 * Housemarque Oy
 * http://www.housemarque.com
 *
 * Permission to copy, use, modify, sell and distribute this software is
 * granted provided this copyright notice appears in all copies. This
 * software is provided "as is" without express or implied warranty, and
 * with no claim as to its suitability for any purpose.
 *
 * See http://www.boost.org for most recent version.
 */

#include <boost/preprocessor/logical/bool.hpp>

/**
Expands to the logical NEITHER OR of the operands.

Both X and Y must expand to integer literals
in the range [0, BOOST_PP_LIMIT_MAG].

For example, BOOST_PP_NOR(0,5) expands to 0 (a single token).

Test

logical_test.cpp

*/
#define BOOST_PP_NOR(X,Y) BOOST_PP_NOR_BOOL(BOOST_PP_BOOL(X),BOOST_PP_BOOL(Y))

#define BOOST_PP_NOR_BOOL(X,Y) BOOST_PP_NOR_BOOL_DELAY(X,Y)
#define BOOST_PP_NOR_BOOL_DELAY(X,Y) BOOST_PP_NOR_BOOL##X##Y
#define BOOST_PP_NOR_BOOL00 1
#define BOOST_PP_NOR_BOOL01 0
#define BOOST_PP_NOR_BOOL10 0
#define BOOST_PP_NOR_BOOL11 0

/*
Obsolete. Use BOOST_PP_NOR().
 */
#define BOOST_PREPROCESSOR_NOR(X,Y) BOOST_PP_NOR(X,Y)
#endif


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

#ifndef MODULES_WORLD_MAP_MAP_INTERFACE_HPP_
#define MODULES_WORLD_MAP_MAP_INTERFACE_HPP_

#include <vector>
#include <string>
#include <utility>
#include <boost/geometry/index/rtree.hpp>

#include "modules/geometry/geometry.hpp"
#include "modules/world/map/map_interface.hpp"
#include "modules/world/map/roadgraph.hpp"

namespace modules {
namespace world {
namespace map {

using modules::world::opendrive::LanePtr;
using modules::world::opendrive::LaneId;
using modules::world::opendrive::OpenDriveMapPtr;
using modules::geometry::Point2d;

using rtree_lane_model = boost::geometry::model::segment<Point2d>;
using rtree_lane_id = LanePtr;
using rtree_lane_value = std::pair<rtree_lane_model, rtree_lane_id>;
using rtree_lane = boost::geometry::index::rtree<rtree_lane_value,
                   boost::geometry::index::linear<16, 4> >;

class MapInterface {
 public:
  bool interface_from_opendrive(const OpenDriveMapPtr& open_drive_map);

  bool FindNearestLanes(const modules::geometry::Point2d& point,
                         const unsigned& num_lanes,
                         std::vector<opendrive::LanePtr>& lanes);

  std::pair< std::vector<LanePtr>, std::vector<LanePtr> > ComputeLaneBoundariesHorizon(const LaneId& startid, const LaneId& goalid);

  std::pair< std::vector<LanePtr>, std::vector<LanePtr> > ComputeLaneBoundaries(const std::vector<LaneId>& horizon);

  bool set_open_drive_map(OpenDriveMapPtr map) {
    open_drive_map_ = map;
    interface_from_opendrive(open_drive_map_);
    return true;
  }

  bool set_roadgraph(RoadgraphPtr roadgraph) {
    roadgraph_ = roadgraph;
    return true;
  }

  OpenDriveMapPtr get_open_drive_map() { return open_drive_map_; }
  RoadgraphPtr get_roadgraph() { return roadgraph_; }

 private:
  OpenDriveMapPtr open_drive_map_;
  RoadgraphPtr roadgraph_;
  rtree_lane rtree_lane_;
};

using MapInterfacePtr = std::shared_ptr<MapInterface>;

}  // namespace map
}  // namespace world
}  // namespace modules

#endif  // MODULES_WORLD_MAP_MAP_INTERFACE_HPP_


#ifndef BOOST_MPL_AUX_VECTOR_ITERATOR_HPP_INCLUDED
#define BOOST_MPL_AUX_VECTOR_ITERATOR_HPP_INCLUDED

// Copyright Aleksey Gurtovoy 2000-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id$
// $Date$
// $Revision$

#include <boost/mpl/vector/aux_/at.hpp>
#include <boost/mpl/iterator_tags.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/advance_fwd.hpp>
#include <boost/mpl/distance_fwd.hpp>
#include <boost/mpl/next.hpp>
#include <boost/mpl/prior.hpp>
#include <boost/mpl/aux_/value_wknd.hpp>
#include <boost/mpl/aux_/config/ctps.hpp>
#include <boost/mpl/aux_/config/workaround.hpp>

namespace boost { namespace mpl {

template<
      typename Vector
    , long n_
    >
struct v_iter
{
    typedef aux::v_iter_tag tag;
    typedef random_access_iterator_tag category;
    typedef typename v_at<Vector,n_>::type type;

    typedef Vector vector_;
    typedef mpl::long_<n_> pos;

};

template<
      typename Vector
    , long n_
    >
struct next< v_iter<Vector,n_> >
{
    typedef v_iter<Vector,(n_ + 1)> type;
};

template<
      typename Vector
    , long n_
    >
struct prior< v_iter<Vector,n_> >
{
    typedef v_iter<Vector,(n_ - 1)> type;
};

template<
      typename Vector
    , long n_
    , typename Distance
    >
struct advance< v_iter<Vector,n_>,Distance>
{
    typedef v_iter<
          Vector
        , (n_ + BOOST_MPL_AUX_NESTED_VALUE_WKND(long, Distance))
        > type;
};

template<
      typename Vector
    , long n_
    , long m_
    >
struct distance< v_iter<Vector,n_>, v_iter<Vector,m_> >
    : mpl::long_<(m_ - n_)>
{
};

}}

#endif // BOOST_MPL_AUX_VECTOR_ITERATOR_HPP_INCLUDED


#include <cassert>
#include <iostream>

#include <CGAL/Arr_tags.h>

#include <boost/mpl/assert.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/if.hpp>

struct Traits1 {
  typedef CGAL::Arr_open_side_tag Left_side_category;
  typedef CGAL::Arr_open_side_tag Bottom_side_category;
  typedef CGAL::Arr_open_side_tag Top_side_category;
  typedef CGAL::Arr_open_side_tag Right_side_category;
};

struct Traits2 {
  typedef CGAL::Arr_open_side_tag Left_side_category;
  typedef CGAL::Arr_closed_side_tag Bottom_side_category;
  typedef CGAL::Arr_open_side_tag Top_side_category;
  typedef CGAL::Arr_open_side_tag Right_side_category;
};

struct Traits3 {
  typedef CGAL::Arr_open_side_tag Left_side_category;
  typedef CGAL::Arr_open_side_tag Bottom_side_category;
  typedef CGAL::Arr_closed_side_tag Top_side_category;
  typedef CGAL::Arr_open_side_tag Right_side_category;
};

struct Traits4 {
  typedef CGAL::Arr_open_side_tag Left_side_category;
  typedef CGAL::Arr_open_side_tag Bottom_side_category;
  typedef CGAL::Arr_open_side_tag Top_side_category;
  typedef CGAL::Arr_closed_side_tag Right_side_category;
};

struct Traits5 {
};

bool dispatch(CGAL::Arr_all_sides_oblivious_tag) {
  return true;
}

bool dispatch(CGAL::Arr_not_all_sides_oblivious_tag) {
  return false;
}

bool dispatch(CGAL::Arr_all_sides_non_open_tag) {
  return true;
}

bool dispatch(CGAL::Arr_not_all_sides_non_open_tag) {
  return false;
}

int main ()
{

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag >::result result1;

  assert(dispatch(result1()));

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag >::result result2;

  assert(!dispatch(result2()));

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag >::result result3;

  assert(!dispatch(result3()));

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag >::result result4;

  assert(!dispatch(result4()));

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag >::result result5;

  assert(!dispatch(result5()));

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_closed_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag >::result result6;

  assert(!dispatch(result6()));

  typedef CGAL::Arr_are_all_sides_oblivious_tag<
    CGAL::Arr_contracted_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_closed_side_tag >::result result7;

  assert(!dispatch(result7()));

  // all non-open

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag >::result open1;

  assert(dispatch(open1()));

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag >::result open2;

  assert(!dispatch(open2()));

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag >::result open3;

  assert(!dispatch(open3()));

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag >::result open4;

  assert(!dispatch(open4()));

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag >::result open5;

  assert(!dispatch(open5()));

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_closed_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag >::result open6;

  assert(!dispatch(open6()));

  typedef CGAL::Arr_are_all_sides_non_open_tag<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_open_side_tag >::result open7;

  assert(!dispatch(open7()));

  // opposite identified tagging

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_closed_side_tag >::result ident1;

  assert(ident1() == true);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_oblivious_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_identified_side_tag >::result ident2;

  assert(ident2() == true);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_closed_side_tag >::result ident3;

  assert(ident3() == true);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag >::result ident4;

  assert(ident4() == true);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_closed_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag >::result ident5;

  assert(ident5() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag >::result ident6;

  assert(ident6() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_contracted_side_tag,
    CGAL::Arr_identified_side_tag >::result ident7;

  assert(ident7() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_oblivious_side_tag >::result ident8;

  assert(ident8() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_closed_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_identified_side_tag >::result ident9;

  assert(ident9() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_open_side_tag >::result ident10;

  assert(ident10() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_open_side_tag,
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_contracted_side_tag,
    CGAL::Arr_open_side_tag >::result ident11;

  assert(ident11() == false);

  typedef CGAL::Arr_sane_identified_tagging<
    CGAL::Arr_identified_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_open_side_tag,
    CGAL::Arr_oblivious_side_tag >::result ident12;

  assert(ident12() == false);

  CGAL_static_assertion(
      (boost::is_same< CGAL::internal::Arr_complete_left_side_category< Traits5 >::Category,
       CGAL::Arr_oblivious_side_tag >::value)
  );
  CGAL_static_assertion(
      (boost::is_same< CGAL::internal::Arr_complete_left_side_category< Traits1 >::Category,
       CGAL::Arr_open_side_tag >::value)
  );

  CGAL_static_assertion(
      (boost::is_same<CGAL::internal::Arr_complete_bottom_side_category< Traits5 >::Category,
       CGAL::Arr_oblivious_side_tag >::value)
  );
  CGAL_static_assertion(
      (boost::is_same<CGAL::internal::Arr_complete_bottom_side_category< Traits1 >::Category,
       CGAL::Arr_open_side_tag >::value)
  );

  CGAL_static_assertion(
      (boost::is_same< CGAL::internal::Arr_complete_top_side_category< Traits5 >::Category,
       CGAL::Arr_oblivious_side_tag >::value)
  );
  CGAL_static_assertion(
      (boost::is_same< CGAL::internal::Arr_complete_top_side_category< Traits1 >::Category,
       CGAL::Arr_open_side_tag >::value)
  );

  CGAL_static_assertion(
      (boost::is_same< CGAL::internal::Arr_complete_right_side_category< Traits5 >::Category,
       CGAL::Arr_oblivious_side_tag >::value)
  );
  CGAL_static_assertion(
      (boost::is_same< CGAL::internal::Arr_complete_right_side_category< Traits1 >::Category,
       CGAL::Arr_open_side_tag >::value)
  );

  return EXIT_SUCCESS;

}


#include <cstdio>
#include <memory>
#include <queue>

#include <polyvec/mc/segment_connectivity.hpp>
#include <polyvec/mc/bit_vector.hpp>

#include <Eigen/Geometry>

NAMESPACE_BEGIN ( polyfit )
NAMESPACE_BEGIN ( mc )

constexpr int SegmentConnectivity::infinite_region;

int
SegmentConnectivity::n_segments() {
    return _n_segments;
}

RasterImageConnectivity&
SegmentConnectivity::raster_image_connectivity() {
    return *_raster_image_connectivity;
}

int
SegmentConnectivity::segment_to_pixelhe_begin ( const int segment_id ) {
    return _segment_to_pixelhe_begin[ segment_id ];
}

int
SegmentConnectivity::segment_to_pixelhe_end ( const int segment_id ) {
    return _segment_to_pixelhe_end[ segment_id ];
}

int
SegmentConnectivity::pixelhe_to_segment ( const int pixelhe_id ) {
    return _pixelhe_to_segment[ pixelhe_id ];
}

void
SegmentConnectivity::get_segment_points ( const int segment_id, Eigen::Matrix2Xi& points, bool& is_closed ) {
    Mesh_connectivity& pixconn = raster_image_connectivity().connectivity();
    //
    const int  heid_begin =  segment_to_pixelhe_begin ( segment_id );
    const int  heid_end =  segment_to_pixelhe_end ( segment_id );
    //
    Mesh_connectivity::Half_edge_iterator he_begin = pixconn.half_edge_at ( heid_begin );
    Mesh_connectivity::Half_edge_iterator he_end = pixconn.half_edge_at ( heid_end );
    Mesh_connectivity::Half_edge_iterator he;

    // Set is closed
    is_closed = he_begin.is_equal ( he_end );

    // Count the number of vertices
    int n_verts = 0;
    he = he_begin;

    do {
        ++n_verts;
        he = he.next();
    } while ( !he.is_equal ( he_end ) );

    if ( !is_closed ) {
        ++n_verts;
    }

    // Now store the vert positions
    points.resize ( 2, n_verts );
    int offset = 0;
    he = he_begin;

    do {
        Eigen::Vector2i pos = raster_image_connectivity().pixel_vertex_index_to_pos ( he.origin().index() );
        points.col ( offset ) = pos;
        ++offset;
        he = he.next();
    } while ( !he.is_equal ( he_end ) );

    if ( !is_closed ) {
        Eigen::Vector2i pos = raster_image_connectivity().pixel_vertex_index_to_pos ( he.origin().index() );
        points.col ( offset ) = pos;
    }

}

void SegmentConnectivity::get_incident_polygons_for_segment(const int segment_id, int& region0, int& region1) {
    int he_begin = segment_to_pixelhe_begin(segment_id);
    Mesh_connectivity::Half_edge_iterator he_it = _raster_image_connectivity->connectivity().half_edge_at(he_begin);
    region0 = he_it.face().index();
    region1 = he_it.twin().face().index();
}

SegmentConnectivity
SegmentConnectivity::build ( RasterImageConnectivity& raster_image_connectivity, const std::vector<JunctionType>& junction_types ) {
    assert_break ( ( int ) junction_types.size() == raster_image_connectivity.n_pixel_vertices() );
    Mesh_connectivity& pixconn = raster_image_connectivity.connectivity();

    // Mappings
    std::vector<int> pixelhe_to_segment ( pixconn.n_active_half_edges(), infinite_region );
    std::vector<int> segment_to_pixelhe_begin ( 0 );
    std::vector<int> segment_to_pixelhe_end ( 0 );
    int n_segments = 0;

    // Detect 3 or 4 junction
    auto is_34junction = [&junction_types] ( const int jt ) {
        return ( junction_types[jt] == JUNCTION_3_SHARP ) || ( junction_types[jt] == JUNCTION_4_SHARP ) ;
    };

    //
    // Push all he that begin at a junction into a stack
    //
    std::queue<int> to_inspect;

    for ( int heid = 0 ; heid < pixconn.n_total_half_edges() ; ++heid ) {
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( heid );
        Mesh_connectivity::Vertex_iterator vert = he.origin();
        Mesh_connectivity::Face_iterator face = he.face();
        assert_break ( vert.is_active() );

        bool is_okay = true;
        is_okay = is_okay && ( !face.is_equal ( pixconn.hole() ) ); // must check before calling last one
        is_okay = is_okay && is_34junction ( vert.index() ) ;
        is_okay = is_okay && ( raster_image_connectivity.get_polygon_type ( face.index() ) == RasterImageConnectivity::POLYGON_OUTER_BOUNDARY );

        if ( is_okay ) {
            to_inspect.push ( he.index() );
        }
    }

    //
    // Traverse half edges and create a segment for them
    //
    while ( !to_inspect.empty() ) {
        const int candidate = to_inspect.front();
        to_inspect.pop();

        //
        // Non-hole half-edges can be visited twice. Prevent it
        if ( pixelhe_to_segment[ candidate ] != infinite_region ) {
            continue;
        }

        //
        // Now create the segment
        const int segment_id = n_segments;
        ++n_segments;
        //
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( candidate );
        Mesh_connectivity::Half_edge_iterator he_begin = he;
        bool should_continue = false;

        int safe_gaurd_counter = 0;
        const int safe_gaurd_max = 10000;

        do {
            // Set maps
            assert_break ( raster_image_connectivity.get_polygon_type ( he.face().index() ) == RasterImageConnectivity::POLYGON_OUTER_BOUNDARY );
            pixelhe_to_segment[ he.index() ] = segment_id;
            pixelhe_to_segment[ he.twin().index() ] = segment_id;
            // Advance
            he = he.next();
            // Should continue ?
            should_continue = true;
            should_continue = should_continue && ( !he.is_equal ( he_begin ) );
            should_continue = should_continue &&  ( !is_34junction ( he.origin().index() ) );
            // Safe gaurd
            ++safe_gaurd_counter;
            assert_break ( safe_gaurd_counter < safe_gaurd_max );
        } while ( should_continue );

        //
        segment_to_pixelhe_begin.push_back ( he_begin.index() );
        segment_to_pixelhe_end.push_back ( he.index() );
    } // done creating segments

    //
    // Now push all non-visited half-edges to a queue
    //
    to_inspect = std::queue<int>();

    for ( int heid = 0 ; heid < pixconn.n_total_half_edges() ; ++heid ) {
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( heid );
        Mesh_connectivity::Vertex_iterator vert = he.origin();
        Mesh_connectivity::Face_iterator face = he.face();

        assert_break ( vert.is_active() );

        bool is_okay = true;
        is_okay = is_okay && ( !face.is_equal ( pixconn.hole() ) ); // must check before next one
        is_okay = is_okay && ( raster_image_connectivity.get_polygon_type ( face.index() ) == RasterImageConnectivity::POLYGON_OUTER_BOUNDARY );
        is_okay = is_okay && ( pixelhe_to_segment[heid] == infinite_region );

        if ( is_okay ) {
            assert_break ( !is_34junction ( vert.index() ) );
            to_inspect.push ( he.index() );
        }
    }

    //
    // Now extract closed loops
    //
    while ( !to_inspect.empty() ) {
        // Get a non-marked candidate
        const int candidate = to_inspect.front();
        to_inspect.pop();

        if ( pixelhe_to_segment[ candidate ] != infinite_region ) {
            continue;
        }

        // Create a segment
        const int segment_id = n_segments;
        ++n_segments;
        //
        // Now create the segment
        Mesh_connectivity::Half_edge_iterator he = pixconn.half_edge_at ( candidate );
        Mesh_connectivity::Half_edge_iterator he_begin = he;
        bool should_continue = false;

        int safe_gaurd_counter = 0;
        const int safe_gaurd_max = 10000;

        do {
            // Set maps
            assert_break ( raster_image_connectivity.get_polygon_type ( he.face().index() ) == RasterImageConnectivity::POLYGON_OUTER_BOUNDARY );
            assert_break ( !is_34junction ( he.origin().index() ) );
            pixelhe_to_segment[ he.index() ] = segment_id;
            pixelhe_to_segment[ he.twin().index() ] = segment_id;
            // Advance
            he = he.next();
            // Should continue ?
            should_continue = true;
            should_continue = should_continue && ( !he.is_equal ( he_begin ) );
            // Safe gaurd
            ++safe_gaurd_counter;
            assert_break ( safe_gaurd_counter < safe_gaurd_max );
        } while ( should_continue );

        //
        segment_to_pixelhe_begin.push_back ( he_begin.index() );
        segment_to_pixelhe_end.push_back ( he.index() );
    } // done creating segments

    //
    // Make sure all half-edges were visited
    //
    for ( int heid = 0 ; heid < pixconn.n_total_half_edges() ; ++heid ) {
        assert_break ( pixelhe_to_segment[heid] != infinite_region );
    }

    //
    // Create output
    //
    SegmentConnectivity buildee;
    buildee._n_segments = n_segments;
    buildee._pixelhe_to_segment = pixelhe_to_segment;
    buildee._segment_to_pixelhe_end = segment_to_pixelhe_end;
    buildee._segment_to_pixelhe_begin = segment_to_pixelhe_begin;
    buildee._raster_image_connectivity = &raster_image_connectivity;
    return buildee;
}

NAMESPACE_END ( mc )
NAMESPACE_END ( polyfit )