// $Id$
/*
 * This is an extensive modification by Greg Landrum of
 * pieces from several files in the vflib-2.0 distribution
 *
 * The initial version of the modifications was completed
 *   in April 2009.
 *
 * the original author of the vflib files is:
 *    Author: P. Foggia
 *  http://amalfi.dis.unina.it/graph/db/vflib-2.0/doc/vflib.html
 *
 */
#include <boost/graph/adjacency_list.hpp>
#include <vector>
#include <algorithm>
#include <cstring>

#ifndef __BGL_VF2_SUB_STATE_H__
#define __BGL_VF2_SUB_STATE_H__

namespace boost{
  namespace detail {
    typedef unsigned short node_id;
    const node_id NULL_NODE=0xFFFF;
    struct NodeInfo {
      node_id id;
      node_id in;
      node_id out;
    };

    /**
     * The ordering by in/out degree
     */
    static bool nodeInfoComp1(const NodeInfo &a, const NodeInfo &b) {
      if(a.out < b.out) return true;
      if(a.out > b.out) return false;
      if(a.in < b.in) return true;
      if(a.in > b.in) return false;
      return false;
    }

    /**
     * The ordering by frequency/valence.
     * The frequency is in the out field, the valence in `in'.
     */
    static int nodeInfoComp2(const NodeInfo &a, const NodeInfo &b) {
      if (!a.in && b.in ) return 1;
      if (a.in && !b.in) return -1;
      if (a.out < b.out) return -1;
      if (a.out > b.out) return 1;
      if( a.in < b.in ) return -1;
      if (a.in > b.in) return 1;
      return 0;
    }

    template <class Graph,class VertexDescr,class EdgeDescr>
    VertexDescr getOtherIdx(const Graph &g,const EdgeDescr &edge,const VertexDescr &vertex) {
      VertexDescr tmp=boost::source(edge,g);
      if(tmp==vertex){
        tmp=boost::target(edge,g);
      }
      return tmp;
    }

    /*----------------------------------------------------
     * Sorts the nodes of a graphs, returning a
     * heap-allocated vector (using new) with the node ids
     * in the proper orders.
     * The sorting criterion takes into account:
     *    1 - The number of nodes with the same in/out
     *        degree.
     *    2 - The valence of the nodes.
     * The nodes at the beginning of the vector are
     * the most singular, from which the matching should
     * start.
     *--------------------------------------------------*/
    template <class Graph>
    node_id* SortNodesByFrequency(const Graph *g) {
      std::vector<NodeInfo> vect;
      vect.reserve(boost::num_vertices(*g));
      typename Graph::vertex_iterator bNode,eNode;
      boost::tie(bNode,eNode) = boost::vertices(*g);
      while(bNode!=eNode){
        NodeInfo t;
        t.id=vect.size();
        t.in=boost::out_degree(*bNode,*g);// <- assuming undirected graph
        t.out=boost::out_degree(*bNode,*g);
        vect.push_back(t);
        ++bNode;
      }
      std::sort(vect.begin(),vect.end(),nodeInfoComp1);

      unsigned int run=1;
      for(unsigned int i=0; i<vect.size(); i+=run){
        for(run=1; i+run<vect.size() &&
              vect[i+run].in==vect[i].in &&
              vect[i+run].out==vect[i].out;
            ++run)
          ;
        for(unsigned int j=0; j<run; ++j) {
          vect[i+j].in += vect[i+j].out;
          vect[i+j].out=run;
        }
      }
      std::sort(vect.begin(),vect.end(),nodeInfoComp2);

      node_id *nodes=new node_id[vect.size()];
      for(unsigned int i=0; i<vect.size(); ++i){
        nodes[i]=vect[i].id;
      }

      return nodes;
    }

    /*----------------------------------------------------------
     * class VF2SubState
     * A representation of the SSS current state
     ---------------------------------------------------------*/
    template <class Graph,class VertexCompatible,class EdgeCompatible,class MatchChecking >
    class VF2SubState
    {
    private:
      Graph *g1, *g2;
      VertexCompatible &vc;
      EdgeCompatible &ec;
      MatchChecking &mc;
      unsigned int n1, n2;

      unsigned int core_len, orig_core_len;
      unsigned int added_node1;
      unsigned int t1both_len, t2both_len;
      unsigned int t1in_len, t1out_len;
      unsigned int t2in_len, t2out_len; // Core nodes are also counted by these...
      node_id *core_1;
      node_id *core_2;
      node_id *in_1;
      node_id *in_2;
      node_id *out_1;
      node_id *out_2;

      node_id *order;

      long *share_count;
      int *vs_compared;

    public:
      VF2SubState(Graph *ag1, Graph *ag2,
                  VertexCompatible &avc,
                  EdgeCompatible &aec,
                  MatchChecking &amc,
                  bool sortNodes=false) : g1(ag1), g2(ag2), vc(avc), ec(aec), mc(amc),
                                          n1(num_vertices(*ag1)),n2(num_vertices(*ag2)) {
        if (sortNodes){
          order = SortNodesByFrequency(ag1);
        } else {
          order = NULL;
        }

        core_len=orig_core_len=0;
        t1both_len=t1in_len=t1out_len=0;
        t2both_len=t2in_len=t2out_len=0;

        added_node1=NULL_NODE;

        core_1=new node_id[n1];
        core_2=new node_id[n2];
        in_1=new node_id[n1];
        in_2=new node_id[n2];
        out_1=new node_id[n1];
        out_2=new node_id[n2];
        share_count = new long;

        for(unsigned int i=0; i<n1; i++){
          core_1[i]=NULL_NODE;
          in_1[i]=0;
          out_1[i]=0;
        }
        for(unsigned int i=0; i<n2; i++){
          core_2[i]=NULL_NODE;
          in_2[i]=0;
          out_2[i]=0;
        }
        vs_compared=0;
        //vs_compared = new int[n1*n2];
        //memset((void *)vs_compared,0,n1*n2*sizeof(int));

        //es_compared = new std::map<unsigned int,bool>();
        *share_count = 1;
      };

      VF2SubState(const VF2SubState &state) :
        g1(state.g1), g2(state.g2), vc(state.vc), ec(state.ec), mc(state.mc),
        n1(state.n1),n2(state.n2), order(state.order),vs_compared(state.vs_compared)
        //es_compared(state.es_compared)
      {

        core_len=orig_core_len=state.core_len;
        t1in_len=state.t1in_len;
        t1out_len=state.t1out_len;
        t1both_len=state.t1both_len;
        t2in_len=state.t2in_len;
        t2out_len=state.t2out_len;
        t2both_len=state.t2both_len;

        added_node1=NULL_NODE;

        core_1=state.core_1;
        core_2=state.core_2;
        in_1=state.in_1;
        in_2=state.in_2;
        out_1=state.out_1;
        out_2=state.out_2;
        share_count=state.share_count;

        ++(*share_count);
      };

      ~VF2SubState(){
        if (-- *share_count == 0) {
          delete [] core_1;
          delete [] core_2;
          delete [] in_1;
          delete [] out_1;
          delete [] in_2;
          delete [] out_2;
          delete share_count;
          delete [] order;
          //delete [] vs_compared;
          //delete es_compared;
        }
      };

      bool IsGoal() { return core_len==n1 ; };
      bool MatchChecks(const node_id c1[],const node_id c2[]){
        return mc(c1,c2);
      };
      bool IsDead() { return n1>n2  ||
          t1both_len>t2both_len ||
          t1out_len>t2out_len ||
          t1in_len>t2in_len;
      };
      unsigned int CoreLen() { return core_len; }
      Graph *GetGraph1() { return g1; }
      Graph *GetGraph2() { return g2; }

      bool NextPair(node_id *pn1, node_id *pn2,
                    node_id prev_n1=NULL_NODE, node_id prev_n2=NULL_NODE){
        if (prev_n1==NULL_NODE)
          prev_n1=0;
        if (prev_n2==NULL_NODE)
          prev_n2=0;
        else
          prev_n2++;

#if 0
    std::cerr<<" **** np: "<< prev_n1<<","<<prev_n2<<std::endl;
    std::cerr<<"in_1 ";
    for(unsigned int i=0;i<n1;++i){
      std::cerr<<"("<<in_1[i]<<","<<out_1[i]<<"), ";
    }
    std::cerr<<std::endl;
    std::cerr<<"in_2 ";
    for(unsigned int i=0;i<n2;++i){
      std::cerr<<"("<<in_2[i]<<","<<out_2[i]<<"), ";
    }
    std::cerr<<std::endl;
#endif
        if (t1both_len>core_len && t2both_len>core_len) {
          while (prev_n1<n1 &&
                 (core_1[prev_n1]!=NULL_NODE || out_1[prev_n1]==0
                  || in_1[prev_n1]==0) ) {
            prev_n1++;
            prev_n2=0;
          }
        }
        else if (t1out_len>core_len && t2out_len>core_len) {
          while (prev_n1<n1 &&
                 (core_1[prev_n1]!=NULL_NODE || out_1[prev_n1]==0) ){
            prev_n1++;
            prev_n2=0;
          }
        }
        else if (t1in_len>core_len && t2in_len>core_len) {
          while (prev_n1<n1 &&
                 (core_1[prev_n1]!=NULL_NODE || in_1[prev_n1]==0) ) {
            prev_n1++;
            prev_n2=0;
          }
        }
        else if (prev_n1==0 && order!=NULL) {
          unsigned int i=0;
          while (i<n1 && core_1[prev_n1=order[i]] != NULL_NODE)
            i++;
          if (i==n1)
            prev_n1=n1;
        }
        else {
          while (prev_n1<n1 && core_1[prev_n1]!=NULL_NODE ){
            prev_n1++;
            prev_n2=0;
          }
        }

        if (t1both_len>core_len && t2both_len>core_len) {
          while (prev_n2<n2 &&
                 (core_2[prev_n2]!=NULL_NODE || out_2[prev_n2]==0
                  || in_2[prev_n2]==0) ) {
            prev_n2++;
          }
        }
        else if (t1out_len>core_len && t2out_len>core_len) {
          while (prev_n2<n2 &&
                 (core_2[prev_n2]!=NULL_NODE || out_2[prev_n2]==0) ) {
            prev_n2++;
          }
        }
        else if (t1in_len>core_len && t2in_len>core_len) {
          while (prev_n2<n2 &&
                 (core_2[prev_n2]!=NULL_NODE || in_2[prev_n2]==0) ) {
            prev_n2++;
          }
        }
        else {
          while (prev_n2<n2 && core_2[prev_n2]!=NULL_NODE ){
            prev_n2++;
          }
        }
        //std::cerr<<" "<< prev_n1<<"<"<<n1<<" "<<prev_n2<<"<"<<n2;
        if (prev_n1<n1 && prev_n2<n2) {
          *pn1=prev_n1;
          *pn2=prev_n2;
          //std::cerr<<"  Found"<<std::endl;
          return true;
        }
        //std::cerr<<"  nope"<< std::endl;
        return false;
      };
      bool IsFeasiblePair(node_id node1, node_id node2){
        assert(node1 < n1);
        assert(node2 < n2);
        assert(core_1[node1] == NULL_NODE);
        assert(core_2[node2] == NULL_NODE);

        //std::cerr<<"  ifp:"<<node1<<"-"<<node2<<" "<<vs_compared->size()<<std::endl;
        // int &isCompat=vs_compared[node1*n2+node2];
        // if(isCompat==0){
        //   isCompat=vc(node1,node2)?1:-1;
        // }
        // if( isCompat<0 ){
        //   //std::cerr<<"  short1"<<std::endl;
        //   return false;
        // }
        if(!vc(node1,node2)) return false;

        unsigned int other1, other2;
        unsigned int termout1 = 0, termout2 = 0, termin1 = 0, termin2 = 0;
        unsigned int new1 = 0, new2 = 0;

        // Check the out edges of node1
        typename Graph::out_edge_iterator bNbrs,eNbrs;
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          other1=getOtherIdx(*g1,*bNbrs,node1);
          if (core_1[other1] != NULL_NODE) {
            other2 = core_1[other1];
            typename Graph::edge_descriptor oEdge;
            bool found;
            boost::tie(oEdge,found) = boost::edge(node2,other2,*g2);
            if(!found || !ec(*bNbrs,oEdge) ){
              //std::cerr<<"  short2"<<std::endl;
              return false;
            }
          } else {
            if (in_1[other1]) ++termin1;
            if (out_1[other1]) ++termout1;
            if (!in_1[other1] && !out_1[other1]) ++new1;
          }
          ++bNbrs;
        }

        // Check the out edges of node2
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          other2=getOtherIdx(*g2,*bNbrs,node2);
          if (core_2[other2] != NULL_NODE) {
            // do nothing
          } else {
            if (in_2[other2]) ++termin2;
            if (out_2[other2]) ++termout2;
            if (!in_2[other2] && !out_2[other2]) ++new2;
          }
          ++bNbrs;
        }
        //std::cerr<<(termin1 <= termin2 && termout1 <= termout2 && (termin1+termout1+new1)<=(termin2+termout2+new2))<<std::endl;

        return termin1 <= termin2 && termout1 <= termout2 && (termin1+termout1+new1)<=(termin2+termout2+new2);
      };
      void AddPair(node_id node1, node_id node2){
        assert(node1 < n1);
        assert(node2 < n2);
        assert(core_len < n1);
        assert(core_len < n2);

        ++core_len;
        added_node1 = node1;

        if (!in_1[node1]) {
          in_1[node1] = core_len;
          ++t1in_len;
          if (out_1[node1]) ++t1both_len;
        }
        if (!out_1[node1]) {
          out_1[node1] = core_len;
          ++t1out_len;
          if (in_1[node1]) ++t1both_len;
        }

        if (!in_2[node2]) {
          in_2[node2] = core_len;
          ++t2in_len;
          if (out_2[node2]) ++t2both_len;
        }
        if (!out_2[node2]) {
          out_2[node2] = core_len;
          ++t2out_len;
          if (in_2[node2]) ++t2both_len;
        }

        core_1[node1] = node2;
        core_2[node2] = node1;

        typename Graph::out_edge_iterator bNbrs,eNbrs;
        // FIX: this is explicitly ignoring directionality
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node1,*g1);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g1,*bNbrs,node1);
          if (!in_1[other]) {
            in_1[other] = core_len;
            ++t1in_len;
            if (out_1[other])  ++t1both_len;
          }
          if (!out_1[other]) {
            out_1[other] = core_len;
            ++t1out_len;
            if (in_1[other])  ++t1both_len;
          }
          ++bNbrs;
        }

        // FIX: this is explicitly ignoring directionality
        boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
        while(bNbrs!=eNbrs){
          unsigned int other = getOtherIdx(*g2,*bNbrs,node2);
          if (!in_2[other]) {
            in_2[other] = core_len;
            ++t2in_len;
            if (out_2[other]) ++t2both_len;
          }
          if (!out_2[other]) {
            out_2[other] = core_len;
            ++t2out_len;
            if (in_2[other]) ++t2both_len;
          }
          ++bNbrs;
        }
      };
      void GetCoreSet(node_id c1[], node_id c2[]){
        unsigned int i, j;
        for (i = 0, j = 0; i < n1; ++i){
          if (core_1[i] != NULL_NODE) {
            c1[j] = i;
            c2[j] = core_1[i];
            ++j;
          }
        }

      };
      VF2SubState *Clone(){
        return new VF2SubState(*this);
      };
      void BackTrack(){
        assert(core_len - orig_core_len <= 1);
        assert(added_node1 != NULL_NODE);

        if (orig_core_len < core_len) {

          if (in_1[added_node1] == core_len) in_1[added_node1] = 0;
          if (out_1[added_node1] == core_len)  out_1[added_node1] = 0;

          typename Graph::out_edge_iterator bNbrs,eNbrs;
          boost::tie(bNbrs,eNbrs) = boost::out_edges(added_node1,*g1);
          while(bNbrs!=eNbrs){
            unsigned int other = getOtherIdx(*g1,*bNbrs,added_node1);
            if (out_1[other] == core_len) out_1[other] = 0;
            if (in_1[other] == core_len) in_1[other] = 0;
            ++bNbrs;
          }

          unsigned int node2 = core_1[added_node1];
          if (in_2[node2] == core_len) in_2[node2] = 0;
          if (out_2[node2] == core_len) out_2[node2] = 0;

          boost::tie(bNbrs,eNbrs) = boost::out_edges(node2,*g2);
          while(bNbrs!=eNbrs){
            unsigned int other = getOtherIdx(*g2,*bNbrs,node2);
            if (out_2[other] == core_len) out_2[other] = 0;
            if (in_2[other] == core_len) in_2[other] = 0;
            ++bNbrs;
          }

          core_1[added_node1] = NULL_NODE;
          core_2[node2] = NULL_NODE;

          core_len = orig_core_len;
          added_node1 = NULL_NODE;
        }
      };
    };

    /*-------------------------------------------------------------
     * static bool match(pn, c1, c2, s)
     * Finds a matching between two graphs, if it exists, starting
     * from state s.
     * Returns true a match has been found.
     * *pn is assigned the numbero of matched nodes, and
     * c1 and c2 will contain the ids of the corresponding nodes
     * in the two graphs.
     ------------------------------------------------------------*/
    template <class SubState>
    bool match(int *pn, node_id c1[], node_id c2[], SubState &s)
    {
      if (s.IsGoal() ) {
        s.GetCoreSet(c1, c2);
        if(s.MatchChecks(c1,c2)) {
          *pn=s.CoreLen();
          return true;
        }
      }

      if (s.IsDead())
        return false;
      //std::cerr<<"  > match: "<<*pn<<" "<<&s<<std::endl;
      node_id n1=NULL_NODE, n2=NULL_NODE;
      bool found=false;
      while (!found && s.NextPair(&n1, &n2, n1, n2)) {
        //std::cerr<<"           "<<n1<<","<<n2<<std::endl;
        if (s.IsFeasiblePair(n1, n2)){
          SubState *s1=s.Clone();
          s1->AddPair(n1, n2);
          found=match(pn, c1, c2, *s1);
          s1->BackTrack();
          delete s1;
        }
      }
      //std::cerr<<"  < returning: "<<found<<" "<<*pn<<" "<<&s<<std::endl;
      return found;
    }

    /*-------------------------------------------------------------
     * static bool match(c1, c2, vis, usr_data, pcount)
     * Visits all the matchings between two graphs,  starting
     * from state s.
     * Returns true if the caller must stop the visit.
     * Stops when there are no more matches
     *
     ------------------------------------------------------------*/
    template <class SubState,class DoubleBackInsertionSequence>
    bool match(node_id c1[], node_id c2[], SubState &s, DoubleBackInsertionSequence &res,
               unsigned int max_results) {
      if (s.IsGoal()){
        s.GetCoreSet(c1, c2);
        if(s.MatchChecks(c1,c2)) {
          typename DoubleBackInsertionSequence::value_type newSeq;
          for(unsigned int i=0;i<s.CoreLen();++i){
            newSeq.push_back(std::pair<int,int>(c1[i],c2[i]));
          }
          res.push_back(newSeq);
          if(res.size()>=max_results) return true;
        }
        return false;
      }

      if (s.IsDead())
        return false;

      node_id n1=NULL_NODE, n2=NULL_NODE;
      while (s.NextPair(&n1, &n2, n1, n2)) {
        if (s.IsFeasiblePair(n1, n2)){
          SubState *s1=s.Clone();
          s1->AddPair(n1, n2);
          if (match(c1, c2, *s1,res,max_results)){
            s1->BackTrack();
            delete s1;
            return true;
          }
          else {
            s1->BackTrack();
            delete s1;
          }
        }
      }
      return false;
    }
  }; //end of namespace detail

  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class MatchChecking      // binary predicate
              , class BackInsertionSequence   // contains std::pair<vertex_descriptor,vertex_descriptor>
              >
  bool vf2(const Graph &g1,const Graph &g2,
           VertexLabeling& vertex_labeling,
           EdgeLabeling& edge_labeling,
           MatchChecking& match_checking,
           BackInsertionSequence& F){
    detail::VF2SubState<const Graph,VertexLabeling,EdgeLabeling,MatchChecking> s0(&g1,&g2,vertex_labeling,
                                                                                edge_labeling,match_checking,false);
    detail::node_id *ni1 = new detail::node_id[num_vertices(g1)];
    detail::node_id *ni2 = new detail::node_id[num_vertices(g2)];
    int n=0;

    F.clear();
    F.resize(0);
    if(match(&n,ni1,ni2,s0)){
      for(unsigned int i=0;i<num_vertices(g1);i++){
        F.push_back(std::pair<int,int>(ni1[i],ni2[i]));
      }
    }
    delete [] ni1;
    delete [] ni2;

    return !F.empty();
  };
  template <  class Graph
              , class VertexLabeling    // binary predicate
              , class EdgeLabeling      // binary predicate
              , class MatchChecking      // binary predicate
              , class DoubleBackInsertionSequence   // contains a back insertion sequence
              >
  bool vf2_all(const Graph& g1, const Graph& g2,
               VertexLabeling& vertex_labeling,
               EdgeLabeling& edge_labeling,
               MatchChecking& match_checking,
               DoubleBackInsertionSequence& F,
               unsigned int max_results=1000) {
    detail::VF2SubState<const Graph,VertexLabeling,EdgeLabeling,MatchChecking> s0(&g1,&g2,vertex_labeling,
                                                                                edge_labeling,match_checking,false);
    detail::node_id *ni1 = new detail::node_id[num_vertices(g1)];
    detail::node_id *ni2 = new detail::node_id[num_vertices(g2)];

    F.clear();
    F.resize(0);

    match(ni1,ni2,s0,F,max_results);

    delete [] ni1;
    delete [] ni2;

    return !F.empty();
  };
} // end of namespace boost
#endif


//
// Copyright (C) 2012-2019  Aleksandar Zlateski <zlateski@mit.edu>
//                          Kisuk Lee           <kisuklee@mit.edu>
// ---------------------------------------------------------------
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//
#pragma once

#include <vector>

#include <boost/pending/disjoint_sets.hpp>

#include "types.hpp"

namespace backend {

template< typename ID, typename F >
inline void
merge_regions( ID const * seg,
               ID * out,
               size_t n,
               F const * dend_values,
               ID const * dend_pairs,
               size_t nedges,
               F threshold )
{
    // Find the largest ID in segmentation
    ID max_segid = seg[0];
    for ( size_t i = 1; i < n; ++i )
    {
        if ( seg[i] > max_segid )
            max_segid = seg[i];
    }

    // Intitialize disjoint set
    std::vector<ID>       p(max_segid+1);
    std::vector<uint8_t>  r(max_segid+1);
    for ( ID i = 0; i < max_segid+1; ++i )
    {
        p[i] = i;
        r[i] = 0;
    }
    boost::disjoint_sets<uint8_t*,ID*> sets(&r[0],&p[0]);

    // Merging
    for ( size_t i = 0; i < nedges; ++i )
    {
        if ( dend_values[i] < threshold )
        {
            break;
        }

        ID s1 = sets.find_set(dend_pairs[2*i]);
        ID s2 = sets.find_set(dend_pairs[2*i+1]);

        if ( s1 != s2 && s1 && s2 )
        {
            sets.link(s1,s2);
            uint32_t s = sets.find_set(s1);
        }
    }

    // Remapping
    std::vector<ID> remaps(max_segid+1);

    ID next_id = 1;

    for ( ID id = 0; id < max_segid+1; ++id )
    {
        ID s = sets.find_set(id);
        if ( s && (remaps[s] == 0) )
        {
            remaps[s] = next_id++;
        }
    }

    for ( std::ptrdiff_t idx = 0; idx < n; ++idx )
    {
        out[idx] = remaps[sets.find_set(seg[idx])];
    }
}

} // namespace backend


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

#include <boost/hana/ext/boost/mpl/vector.hpp>

#include <boost/hana/detail/assert.hpp>

#include <boost/mpl/vector.hpp>
using namespace boost::hana;
namespace mpl = boost::mpl;

int main() {
    BOOST_HANA_CONSTANT_ASSERT(is_empty(mpl::vector<>{}));
    BOOST_HANA_CONSTANT_ASSERT(is_empty(mpl::vector0<>{}));

    BOOST_HANA_CONSTANT_ASSERT(!is_empty(mpl::vector<int>{}));
    BOOST_HANA_CONSTANT_ASSERT(!is_empty(mpl::vector1<int>{}));

    BOOST_HANA_CONSTANT_ASSERT(!is_empty(mpl::vector<int, char>{}));
    BOOST_HANA_CONSTANT_ASSERT(!is_empty(mpl::vector2<int, char>{}));
}


#ifndef BAGGING_UTILS
#define BAGGING_UTILS

#include <vector>
#include <armadillo>

#include "types.hpp"

namespace BaggingUtils {

typedef std::vector<std::vector<int>> std_matrix;
/*
    A struct to store the sampled data in. If each of the vectors
    contains more than one item, than it means that the class
    balance is being preserved, and data is separated into
    positive and negative data.
    * THE ORDER IS ALWAYS (1) NEGATIVE, (2) POSITIVE
*/
struct sampled_data {
    std::vector<Matrix> x_train;
    std::vector<Vector> y_train;
    std::vector<Matrix> x_valid;
    std::vector<Vector> y_valid;
};

/*
    Draws a random reservoir sample of size k
*/
std::vector<int> reservoir_sample ( const int, const std::vector<int>& );

std::vector<int> sampling_without_replacement ( const int, std::vector<int> );

/*
    Splits data into a training and a validation set. (no balance)
*/
sampled_data split_data ( const Matrix&, const double, const bool = true );

/*
    BFS
*/
int bfs ( std_matrix&, int, int );

/*
    Ford-Fulkerson algorithm for Maximum Network Flow
*/
int max_flow ( std_matrix&, int, int, std_matrix& );

}

#endif // BAGGING_UTILS


// Copyright 2014 Jonathan Graehl - http://graehl.org/
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef GRAEHL_SHARED__REDUCE_HPP
#define GRAEHL_SHARED__REDUCE_HPP

#include <boost/range.hpp>
#include <boost/iterator/reverse_iterator.hpp>

/* foldl - see http://en.wikipedia.org/wiki/Fold_(higher-order_function)

   explicit zero version (foldl) allows any left (result) type

   to save typing and allow default zero construction, reduce is foldl but assumes
   result type = sequence type

*/

namespace graehl {

// (1,2) -> (0+1)+2
template <class I, class Op, class Zero>
Zero
foldl(I i, I const& end, Op const& op, Zero z)
{
  for (; i!=end; ++i)
    z = op(z, *i);
  return z;
}

template <class R, class Op, class Zero>
Zero
foldl(R const& range, Op const& op, Zero z)
{
  return foldl(boost::begin(range), boost:end(range), op, z);
}

// unlike foldr, changes associativity of op, without changing order of iteration
// (1,2) -> 2+(1+0)
template <class I, class Op, class Zero>
Zero
foldr_reversed(I i, I const& end, Op const& op, Zero z)
{
  for (; i!=end; ++i)
    z = op(*i, z);
  return z;
}

// (1,2) -> 1+(2+0)
template <class I, class Op, class Zero>
Zero
foldr(I i, I const& end, Op const& op, Zero z)
{
  boost::reverse_iterator<I> ri(end), re(i);
  return foldr_reversed(ri, re, op, z);
}

template <class R, class Op, class Zero>
Zero
foldr(R const& range, Op const& op, Zero z)
{
  return foldr_reversed(boost::rbegin(range), boost:rend(range), op, z);
}

// all of what follows is for default zero w/ same type as sequence:

template <Op2, class V>
struct op2_result
{
  typedef typename boost:result_of<Op2(V, V)>::type type;
};

template <Op2, class I>
struct op2_iterator_result
{
  typedef typename op2_result<Op2, boost::iterator_value<I>::type>
};

template <Op2, class I>
struct op2_range_result
{
  typedef typename op2_result<Op2, boost::range_value<I>::type>
};

template <class I, class Op>
typename op2_iterator_result<Op, I>::type
reduce(I i, I const& end, Op const& op
       , typename op2_iterator_result<Op, I>::type z =
       typename op2_iterator_result<Op, I>::type()
       )
{
  /*
    for (;i!=end;++i)
    z=op(z,*i);
    return z;
  */
  return foldl(i, end, op, z);
}

template <class R, class Op>
typename op2_range_result<Op, R>::type
reduce(R const& range, Op const& op
       , typename op2_range_result<Op, R>::type z =
       typename op2_range_result<Op, R>::type()
       )
{
  //    return reduce(boost::begin(range),boost:end(range),op,z);
  return foldl(range, op, z);
}

}

#endif


// Copyright Raimar Sandner 2012–2020. Distributed under the Boost Software License, Version 1.0. (See accompanying file LICENSE.txt)
#include "MomentumCorrelation.h"
#include <boost/assign.hpp>

using namespace boost::assign;

MomentumCorrelation::MomentumCorrelation(particle::Ptr p0, particle::Ptr p1) :
  structure::Interaction<2>({p0,p1},{},{}),
  EA_Base("MomentumCorrelation",{"<P1P2>"})
{}

const MomentumCorrelation::Averages MomentumCorrelation::average_v(Time, const LazyDensityOperator& matrix) const
{
  auto averages(initializedAverages());

  size_t dim1 = matrix.getDimensions()[0];
  size_t dim2 = matrix.getDimensions()[1];

  for (int n=0; n < dim1; n++)
    for (int m=0; m < dim2;m++) {
      averages(0)+=(-(dim1/2.)+n)*(-(dim2/2.)+m)*matrix(n,m);
    }
  return averages;
}


#include "MeshCuboidTrainer.h"

#include "Utilities.h"

#include <deque>
#include <fstream>
#include <iostream>
#include <sstream>
#include <Eigen/Core>
#include <Eigen/LU>
#include <QFileInfo>

Eigen::MatrixXd regularized_inverse(const Eigen::MatrixXd& _mat)
{
	return (_mat + 1.0E-3 * Eigen::MatrixXd::Identity(_mat.rows(), _mat.cols())).inverse();
}

MeshCuboidTrainer::MeshCuboidTrainer()
{

}

void MeshCuboidTrainer::clear()
{
	for (std::vector< std::list<MeshCuboidFeatures *> >::iterator f_it = feature_list_.begin();
		f_it != feature_list_.end(); ++f_it)
	{
		for (std::list<MeshCuboidFeatures *>::iterator f_jt = (*f_it).begin();
			f_jt != (*f_it).end(); ++f_jt)
			delete (*f_jt);
		(*f_it).clear();
	}
	feature_list_.clear();

	for (std::vector< std::list<MeshCuboidTransformation *> >::iterator t_it = transformation_list_.begin();
		t_it != transformation_list_.end(); ++t_it)
	{
		for (std::list<MeshCuboidTransformation *>::iterator t_jt = (*t_it).begin();
			t_jt != (*t_it).end(); ++t_jt)
			delete (*t_jt);
		(*t_it).clear();
	}
	transformation_list_.clear();
}

bool MeshCuboidTrainer::load_object_list(const std::string &_filename)
{
	std::ifstream file(_filename);
	if (!file)
	{
		std::cerr << "Can't load file: \"" << _filename << "\"" << std::endl;
		return false;
	}

	object_list_.clear();
	std::string buffer;

	while (!file.eof())
	{
		std::getline(file, buffer);
		if (buffer == "") break;
		object_list_.push_back(buffer);
	}

	return true;
}

bool MeshCuboidTrainer::load_features(const std::string &_filename_prefix)
{
	for (std::vector< std::list<MeshCuboidFeatures *> >::iterator f_it = feature_list_.begin();
		f_it != feature_list_.end(); ++f_it)
	{
		for (std::list<MeshCuboidFeatures *>::iterator f_jt = (*f_it).begin();
			f_jt != (*f_it).end(); ++f_jt)
			delete (*f_jt);
		(*f_it).clear();
	}
	feature_list_.clear();

	for (unsigned int cuboid_index = 0; true; ++cuboid_index)
	{
		std::stringstream sstr;
		sstr << _filename_prefix << cuboid_index << std::string(".csv");
		std::string attributes_filename = sstr.str();

		QFileInfo attributes_file(attributes_filename.c_str());
		if (!attributes_file.exists())
			break;

		std::cout << "Loading '" << attributes_filename << "'..." << std::endl;

		std::list<MeshCuboidFeatures *> stats;
		MeshCuboidFeatures::load_feature_collection(
			attributes_filename.c_str(), stats);

		feature_list_.push_back(stats);
	}

	return true;
}

bool MeshCuboidTrainer::load_transformations(const std::string &_filename_prefix)
{
	for (std::vector< std::list<MeshCuboidTransformation *> >::iterator t_it = transformation_list_.begin();
		t_it != transformation_list_.end(); ++t_it)
	{
		for (std::list<MeshCuboidTransformation *>::iterator t_jt = (*t_it).begin();
			t_jt != (*t_it).end(); ++t_jt)
			delete (*t_jt);
		(*t_it).clear();
	}
	transformation_list_.clear();

	for (unsigned int cuboid_index = 0; true; ++cuboid_index)
	{
		std::stringstream  transformation_filename_sstr;
		transformation_filename_sstr << _filename_prefix << cuboid_index << std::string(".csv");

		QFileInfo transformation_file(transformation_filename_sstr.str().c_str());
		if (!transformation_file.exists())
			break;

		std::cout << "Loading '" << transformation_filename_sstr.str() << "'..." << std::endl;

		std::list<MeshCuboidTransformation *> stats;
		MeshCuboidTransformation::load_transformation_collection(
			transformation_filename_sstr.str().c_str(), stats);

		transformation_list_.push_back(stats);
	}

	return true;
}

void MeshCuboidTrainer::get_joint_normal_relations(
	std::vector< std::vector<MeshCuboidJointNormalRelations *> > &_relations,
	const std::list<std::string> *_ignored_object_list) const
{
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;

	unsigned int num_labels = feature_list_.size();
	assert(transformation_list_.size() == num_labels);

	for (std::vector< std::vector<MeshCuboidJointNormalRelations *> >::iterator it_1 = _relations.begin(); it_1 != _relations.end(); ++it_1)
		for (std::vector<MeshCuboidJointNormalRelations *>::iterator it_2 = (*it_1).begin(); it_2 != (*it_1).end(); ++it_2)
			delete (*it_2);

	_relations.clear();
	_relations.resize(num_labels);
	for (unsigned int cuboid_index = 0; cuboid_index < num_labels; ++cuboid_index)
		_relations[cuboid_index].resize(num_labels, NULL);

	for (unsigned int label_index_1 = 0; label_index_1 < num_labels; ++label_index_1)
	{
		for (unsigned int label_index_2 = 0; label_index_2 < num_labels; ++label_index_2)
		{
			//if (label_index_1 == label_index_2) continue;

			// NOTE:
			// 'object_list_' should contain all object names.
			assert(object_list_.size() == feature_list_[label_index_1].size());
			assert(object_list_.size() == feature_list_[label_index_2].size());
			assert(object_list_.size() == transformation_list_[label_index_1].size());
			assert(object_list_.size() == transformation_list_[label_index_2].size());

			std::vector<MeshCuboidFeatures *> feature_1;
			std::vector<MeshCuboidFeatures *> feature_2;
			std::vector<MeshCuboidTransformation *> transformation_1;
			std::vector<MeshCuboidTransformation *> transformation_2;

			feature_1.reserve(feature_list_[label_index_1].size());
			feature_2.reserve(feature_list_[label_index_2].size());
			transformation_1.reserve(transformation_list_[label_index_1].size());
			transformation_2.reserve(transformation_list_[label_index_2].size());

			std::list<std::string>::const_iterator o_it = object_list_.begin();
			std::list<MeshCuboidFeatures *>::const_iterator f_it_1 = feature_list_[label_index_1].begin();
			std::list<MeshCuboidFeatures *>::const_iterator f_it_2 = feature_list_[label_index_2].begin();
			std::list<MeshCuboidTransformation *>::const_iterator t_it_1 = transformation_list_[label_index_1].begin();
			std::list<MeshCuboidTransformation *>::const_iterator t_it_2 = transformation_list_[label_index_2].begin();

			int num_objects = 0;
			while (true)
			{
				if (f_it_1 == feature_list_[label_index_1].end()
					|| f_it_2 == feature_list_[label_index_2].end()
					|| t_it_1 == transformation_list_[label_index_1].end()
					|| t_it_2 == transformation_list_[label_index_2].end())
					break;

				bool has_values = (!(*f_it_1)->has_nan() && !(*f_it_2)->has_nan());

				if (has_values && _ignored_object_list)
				{
					// Check whether the current object should be ignored.
					for (std::list<std::string>::const_iterator io_it = _ignored_object_list->begin();
						io_it != _ignored_object_list->end(); ++io_it)
					{
						if ((*o_it) == (*io_it))
						{
							std::cout << "Mesh [" << (*o_it) << "] is ignored." << std::endl;
							has_values = false;
							break;
						}
					}
				}

				if (has_values)
				{
					feature_1.push_back(*f_it_1);
					feature_2.push_back(*f_it_2);
					transformation_1.push_back(*t_it_1);
					transformation_2.push_back(*t_it_2);
					++num_objects;
				}

				++o_it;
				++f_it_1;
				++f_it_2;
				++t_it_1;
				++t_it_2;
			}

			if (num_objects == 0) continue;

			_relations[label_index_1][label_index_2] = new MeshCuboidJointNormalRelations();
			MeshCuboidJointNormalRelations *relation_12 = _relations[label_index_1][label_index_2];
			assert(relation_12);

			assert(feature_1.size() == num_objects);
			assert(feature_2.size() == num_objects);
			assert(transformation_1.size() == num_objects);
			assert(transformation_2.size() == num_objects);

			// NOTE:
			// Since the center point is always the origin in the local coordinates,
			// it is not used as the feature values.
			const unsigned int num_cols = MeshCuboidJointNormalRelations::k_mat_size;
			Eigen::MatrixXd X(num_objects, num_cols);

			for (int object_index = 0; object_index < num_objects; ++object_index)
			{
				assert(feature_1[object_index]);
				assert(feature_2[object_index]);
				assert(transformation_1[object_index]);
				assert(transformation_2[object_index]);

				Eigen::VectorXd pairwise_feature_vec;
				MeshCuboidJointNormalRelations::get_pairwise_cuboid_features(
					(*feature_1[object_index]), (*feature_2[object_index]),
					transformation_1[object_index], transformation_2[object_index],
					pairwise_feature_vec);

				X.row(object_index) = pairwise_feature_vec;
			}

			Eigen::RowVectorXd mean = X.colwise().mean();
			Eigen::MatrixXd centered_X = X.rowwise() - mean;

			Eigen::MatrixXd cov = (centered_X.transpose() * centered_X) / static_cast<double>(num_objects);
			Eigen::MatrixXd inv_cov = regularized_inverse(cov);

			relation_12->set_mean(mean.transpose());
			relation_12->set_inv_cov(inv_cov);

#ifdef DEBUG_TEST
			Eigen::MatrixXd diff = (X.rowwise() - mean).transpose();
			Eigen::VectorXd error = (diff.transpose() * inv_cov * diff).diagonal();
			std::cout << "(" << label_index_1 << ", " << label_index_2 << "): max_error = " << error.maxCoeff() << std::endl;
#endif
		}
	}
}

void MeshCuboidTrainer::get_cond_normal_relations(
	std::vector< std::vector<MeshCuboidCondNormalRelations *> > &_relations,
	const std::list<std::string> *_ignored_object_list) const
{
	const unsigned int num_features = MeshCuboidFeatures::k_num_features;
	const unsigned int num_global_feature_values = MeshCuboidFeatures::k_num_global_feature_values;

	unsigned int num_labels = feature_list_.size();
	assert(transformation_list_.size() == num_labels);

	for (std::vector< std::vector<MeshCuboidCondNormalRelations *> >::iterator it_1 = _relations.begin(); it_1 != _relations.end(); ++it_1)
		for (std::vector<MeshCuboidCondNormalRelations *>::iterator it_2 = (*it_1).begin(); it_2 != (*it_1).end(); ++it_2)
			delete (*it_2);

	_relations.clear();
	_relations.resize(num_labels);
	for (unsigned int cuboid_index = 0; cuboid_index < num_labels; ++cuboid_index)
		_relations[cuboid_index].resize(num_labels, NULL);

	for (unsigned int label_index_1 = 0; label_index_1 < num_labels; ++label_index_1)
	{
		for (unsigned int label_index_2 = 0; label_index_2 < num_labels; ++label_index_2)
		{
			if (label_index_1 == label_index_2) continue;

			// NOTE:
			// 'object_list_' should contain all object names.
			assert(object_list_.size() == feature_list_[label_index_1].size());
			assert(object_list_.size() == feature_list_[label_index_2].size());
			assert(object_list_.size() == transformation_list_[label_index_1].size());
			assert(object_list_.size() == transformation_list_[label_index_2].size());

			std::vector<MeshCuboidFeatures *> feature_1;
			std::vector<MeshCuboidFeatures *> feature_2;
			std::vector<MeshCuboidTransformation *> transformation_1;
			std::vector<MeshCuboidTransformation *> transformation_2;

			feature_1.reserve(feature_list_[label_index_1].size());
			feature_2.reserve(feature_list_[label_index_2].size());
			transformation_1.reserve(transformation_list_[label_index_1].size());
			transformation_2.reserve(transformation_list_[label_index_2].size());

			std::list<std::string>::const_iterator o_it = object_list_.begin();
			std::list<MeshCuboidFeatures *>::const_iterator f_it_1 = feature_list_[label_index_1].begin();
			std::list<MeshCuboidFeatures *>::const_iterator f_it_2 = feature_list_[label_index_2].begin();
			std::list<MeshCuboidTransformation *>::const_iterator t_it_1 = transformation_list_[label_index_1].begin();
			std::list<MeshCuboidTransformation *>::const_iterator t_it_2 = transformation_list_[label_index_2].begin();

			int num_objects = 0;
			while (true)
			{
				if (f_it_1 == feature_list_[label_index_1].end()
					|| f_it_2 == feature_list_[label_index_2].end()
					|| t_it_1 == transformation_list_[label_index_1].end()
					|| t_it_2 == transformation_list_[label_index_2].end())
					break;

				bool has_values = (!(*f_it_1)->has_nan() && !(*f_it_2)->has_nan());

				if (has_values && _ignored_object_list)
				{
					// Check whether the current object should be ignored.
					for (std::list<std::string>::const_iterator io_it = _ignored_object_list->begin();
						io_it != _ignored_object_list->end(); ++io_it)
					{
						if ((*o_it) == (*io_it))
						{
							std::cout << "Mesh [" << (*o_it) << "] is ignored." << std::endl;
							has_values = false;
							break;
						}
					}
				}

				if (has_values)
				{
					feature_1.push_back(*f_it_1);
					feature_2.push_back(*f_it_2);
					transformation_1.push_back(*t_it_1);
					transformation_2.push_back(*t_it_2);
					++num_objects;
				}

				++o_it;
				++f_it_1;
				++f_it_2;
				++t_it_1;
				++t_it_2;
			}

			if (num_objects == 0) continue;

			_relations[label_index_1][label_index_2] = new MeshCuboidCondNormalRelations();
			MeshCuboidCondNormalRelations *relation_12 = _relations[label_index_1][label_index_2];
			assert(relation_12);

			assert(feature_1.size() == num_objects);
			assert(feature_2.size() == num_objects);
			assert(transformation_1.size() == num_objects);

			Eigen::MatrixXd X_1(num_objects, num_global_feature_values);
			Eigen::MatrixXd X_2(num_objects, num_features);

			for (int object_index = 0; object_index < num_objects; ++object_index)
			{
				assert(feature_1[object_index]);
				assert(feature_2[object_index]);
				assert(transformation_1[object_index]);
				assert(transformation_2[object_index]);

				Eigen::VectorXd global_features_vec_1, transformed_features_vec_12;
				MeshCuboidCondNormalRelations::get_pairwise_cuboid_features(
					(*feature_1[object_index]), (*feature_2[object_index]),
					transformation_1[object_index], transformation_2[object_index],
					global_features_vec_1, transformed_features_vec_12);

				X_1.row(object_index) = global_features_vec_1;
				X_2.row(object_index) = transformed_features_vec_12;
			}

			Eigen::RowVectorXd mean_1 = X_1.colwise().mean();
			Eigen::RowVectorXd mean_2 = X_2.colwise().mean();

			//Eigen::MatrixXd centered_X_1 = X_1.rowwise() - mean_1;
			//Eigen::MatrixXd centered_X_2 = X_2.rowwise() - mean_2;

			//Eigen::MatrixXd cov_11 = (centered_X_1.transpose() * centered_X_1) / static_cast<double>(num_objects);
			//Eigen::MatrixXd cov_22 = (centered_X_2.transpose() * centered_X_2) / static_cast<double>(num_objects);
			//Eigen::MatrixXd cov_12 = (centered_X_1.transpose() * centered_X_2) / static_cast<double>(num_objects);
			//Eigen::MatrixXd cov_21 = (centered_X_2.transpose() * centered_X_1) / static_cast<double>(num_objects);
			//Eigen::MatrixXd inv_cov_11 = regularized_inverse(cov_11);

			//Eigen::MatrixXd conditional_mean_A = cov_21 * inv_cov_11;
			//Eigen::VectorXd conditional_mean_b = mean_2.transpose() - conditional_mean_A * mean_1.transpose();

			//Eigen::MatrixXd conditional_cov_21 = cov_22 - cov_21 * inv_cov_11 * cov_12;
			//Eigen::MatrixXd conditional_inv_cov_21 = regularized_inverse(conditional_cov_21);

			// http://www.rni.helsinki.fi/~jmh/mrf08/helsinki-1.pdf, page 41.
			Eigen::MatrixXd X(num_objects, num_global_feature_values + num_features);
			X << X_1, X_2;

			Eigen::RowVectorXd mean = X.colwise().mean();
			Eigen::MatrixXd centered_X = X.rowwise() - mean;

			Eigen::MatrixXd cov = (centered_X.transpose() * centered_X) / static_cast<double>(num_objects);
			Eigen::MatrixXd inv_cov = regularized_inverse(cov);

			Eigen::MatrixXd inv_cov_22 = inv_cov.block(
				num_global_feature_values, num_global_feature_values, num_features, num_features);

			Eigen::MatrixXd inv_cov_21 = inv_cov.block(
				num_global_feature_values, 0, num_features, num_global_feature_values);

			Eigen::MatrixXd conditional_mean_A = regularized_inverse(inv_cov_22) * inv_cov_21;
			Eigen::VectorXd conditional_mean_b = mean_2.transpose() - conditional_mean_A * mean_1.transpose();
			Eigen::MatrixXd conditional_inv_cov_21 = inv_cov_22;

			relation_12->set_mean_A(conditional_mean_A);
			relation_12->set_mean_b(conditional_mean_b);
			relation_12->set_inv_cov(conditional_inv_cov_21);

#ifdef DEBUG_TEST
			Eigen::MatrixXd mean_12 = (conditional_mean_A * X_1.transpose()).colwise() + conditional_mean_b;
			Eigen::MatrixXd diff = (X_2.transpose() - mean_12);
			Eigen::VectorXd error = (diff.transpose() * conditional_inv_cov_21 * diff).diagonal();
			std::cout << "(" << label_index_1 << ", " << label_index_2 << "): max_error = " << error.maxCoeff() << std::endl;
#endif
		}
	}
}

void MeshCuboidTrainer::get_conflicted_labels(std::vector< std::list<LabelIndex> > &_conflicted_labels)const
{
	unsigned int num_labels = feature_list_.size();
	assert(transformation_list_.size() == num_labels);

	_conflicted_labels.clear();
	_conflicted_labels.resize(num_labels);

	for (unsigned int label_index_1 = 0; label_index_1 < num_labels; ++label_index_1)
	{
		for (unsigned int label_index_2 = 0; label_index_2 < num_labels; ++label_index_2)
		{
			if (label_index_1 == label_index_2) continue;

			// NOTE:
			// 'object_list_' should contain all object names.
			assert(object_list_.size() == feature_list_[label_index_1].size());
			assert(object_list_.size() == feature_list_[label_index_2].size());

			std::list<MeshCuboidFeatures *>::const_iterator f_it_1 = feature_list_[label_index_1].begin();
			std::list<MeshCuboidFeatures *>::const_iterator f_it_2 = feature_list_[label_index_2].begin();

			int num_objects = 0;
			while (true)
			{
				if (f_it_1 == feature_list_[label_index_1].end()
					|| f_it_2 == feature_list_[label_index_2].end())
					break;

				bool has_values = (!(*f_it_1)->has_nan() && !(*f_it_2)->has_nan());

				if (has_values)
					++num_objects;

				++f_it_1;
				++f_it_2;
			}

			// NOTE: If both labels have never appeared simultaneously in any object,
			// they are defined as conflicted labels.
			if (num_objects == 0)
				_conflicted_labels[label_index_1].push_back(label_index_2);
		}
	}
}

void MeshCuboidTrainer::get_missing_label_index_groups(
	const std::list<LabelIndex> &_given_label_indices,
	std::list< std::list<LabelIndex> > &_missing_label_index_groups,
	const std::set<LabelIndex> *_ignored_label_indices)const
{
	unsigned int num_labels = feature_list_.size();
	assert(transformation_list_.size() == num_labels);

	std::vector< std::list<LabelIndex> > conflicted_labels;
	get_conflicted_labels(conflicted_labels);
	assert(conflicted_labels.size() == num_labels);

	// Consider conflicted labels of the given label indices.
	bool *is_label_missing = new bool[num_labels];
	memset(is_label_missing, true, num_labels*sizeof(bool));

	for (std::list<LabelIndex>::const_iterator it = _given_label_indices.begin();
		it != _given_label_indices.end(); ++it)
	{
		LabelIndex label_index = (*it);
		assert(label_index < num_labels);

		// Ignore the given labels.
		is_label_missing[label_index] = false;

		// Ignore conflicted labels of the given labels.
		for (std::list<LabelIndex>::const_iterator n_it = conflicted_labels[label_index].begin();
			n_it != conflicted_labels[label_index].end(); ++n_it)
		{
			LabelIndex n_label_index = (*n_it);
			assert(n_label_index < num_labels);
			is_label_missing[n_label_index] = false;
		}
	}

	// Clustering missing label indices.

	// Any missing label should be included at least once in any of the missing label groups.
	bool *is_label_not_included = new bool[num_labels];
	memcpy(is_label_not_included, is_label_missing, num_labels*sizeof(bool));
	_missing_label_index_groups.clear();

	while (true)
	{
		// Find a label which is never included in any of the missing label groups.
		LabelIndex seed_label_index = 0;
		for (; seed_label_index < num_labels; ++seed_label_index)
			if (is_label_not_included[seed_label_index])
				break;

		if (seed_label_index == num_labels) break;
		//printf("- seed : %d\n", seed_label_index);

		// Any missing label can be added
		// (even though it has been in included in other missing label groups).
		bool *is_label_available = new bool[num_labels];
		memcpy(is_label_available, is_label_missing, num_labels*sizeof(bool));
		std::list<LabelIndex> missing_label_indices;

		std::list<LabelIndex> label_queue;
		label_queue.push_back(seed_label_index);

		// Add all labels in to the queue.
		// The order does not matter except the seed.
		// The seed should be first since it must be added.
		for (LabelIndex label_index = 0; label_index < num_labels; ++label_index)
			if (label_index != seed_label_index)
				label_queue.push_back(label_index);

		for (std::list<LabelIndex>::iterator it = label_queue.begin(); it != label_queue.end(); ++it)
		{
			LabelIndex label_index = (*it);

			if (is_label_available[label_index])
			{
				missing_label_indices.push_back(label_index);
				//printf("- added : %d\n", label_index);

				is_label_available[label_index] = false;
				// The label is included in a group.
				is_label_not_included[label_index] = false;

				// Any conflicted label becomes unavailable.
				for (std::list<LabelIndex>::const_iterator n_it = conflicted_labels[label_index].begin();
					n_it != conflicted_labels[label_index].end(); ++n_it)
				{
					LabelIndex n_label_index = (*n_it);
					assert(n_label_index < num_labels);
					is_label_available[n_label_index] = false;
				}
			}
		}

		if (_ignored_label_indices)
		{
			// Remove ignored label indices.
			for (std::list<LabelIndex>::iterator it = missing_label_indices.begin();
				it != missing_label_indices.end();) // No increment.
			{
				bool exist = false;
				for (std::set<LabelIndex>::const_iterator jt = (*_ignored_label_indices).begin();
					jt != (*_ignored_label_indices).end(); ++jt)
				{
					if ((*it) == (*jt))
					{
						exist = true;
						break;
					}
				}
				if (exist)
					it = missing_label_indices.erase(it);
				else
					++it;
			}
		}

		if (!missing_label_indices.empty())
			_missing_label_index_groups.push_back(missing_label_indices);

		delete[] is_label_available;
	}

	delete[] is_label_not_included;
	delete[] is_label_missing;
}

void MeshCuboidTrainer::load_joint_normal_relations(
	const unsigned int _num_labels, const std::string _filename_prefix,
	std::vector< std::vector<MeshCuboidJointNormalRelations *> > &_relations)
{
	for (std::vector< std::vector<MeshCuboidJointNormalRelations *> >::iterator it_1 = _relations.begin(); it_1 != _relations.end(); ++it_1)
		for (std::vector<MeshCuboidJointNormalRelations *>::iterator it_2 = (*it_1).begin(); it_2 != (*it_1).end(); ++it_2)
			delete (*it_2);

	_relations.clear();
	_relations.resize(_num_labels);

	for (LabelIndex label_index_1 = 0; label_index_1 < _num_labels; ++label_index_1)
	{
		_relations[label_index_1].resize(_num_labels, NULL);
		for (LabelIndex label_index_2 = 0; label_index_2 < _num_labels; ++label_index_2)
		{
			if (label_index_1 == label_index_2)
				continue;

			std::stringstream relation_filename_sstr;
			relation_filename_sstr << _filename_prefix
				<< label_index_1 << std::string("_")
				<< label_index_2 << std::string(".csv");

			QFileInfo relation_file(relation_filename_sstr.str().c_str());
			if (!relation_file.exists()) continue;

			_relations[label_index_1][label_index_2] = new MeshCuboidJointNormalRelations();
			bool ret = _relations[label_index_1][label_index_2]->load_joint_normal_csv(
				relation_filename_sstr.str().c_str());

			if (!ret)
			{
				do {
					std::cout << '\n' << "Press the Enter key to continue.";
				} while (std::cin.get() != '\n');
			}
		}
	}
}

void MeshCuboidTrainer::load_cond_normal_relations(
	const unsigned int _num_labels, const std::string _filename_prefix,
	std::vector< std::vector<MeshCuboidCondNormalRelations *> > &_relations)
{
	for (std::vector< std::vector<MeshCuboidCondNormalRelations *> >::iterator it_1 = _relations.begin(); it_1 != _relations.end(); ++it_1)
		for (std::vector<MeshCuboidCondNormalRelations *>::iterator it_2 = (*it_1).begin(); it_2 != (*it_1).end(); ++it_2)
			delete (*it_2);

	_relations.clear();
	_relations.resize(_num_labels);

	for (LabelIndex label_index_1 = 0; label_index_1 < _num_labels; ++label_index_1)
	{
		_relations[label_index_1].resize(_num_labels, NULL);
		for (LabelIndex label_index_2 = 0; label_index_2 < _num_labels; ++label_index_2)
		{
			if (label_index_1 == label_index_2)
				continue;

			std::stringstream relation_filename_sstr;
			relation_filename_sstr << std::string("")
				<< label_index_1 << std::string("_")
				<< label_index_2 << std::string(".csv");

			QFileInfo relation_file(relation_filename_sstr.str().c_str());
			if (!relation_file.exists()) continue;

			_relations[label_index_1][label_index_2] = new MeshCuboidCondNormalRelations();
			bool ret = _relations[label_index_1][label_index_2]->load_cond_normal_csv(
				relation_filename_sstr.str().c_str());
			if (!ret)
			{
				do {
					std::cout << '\n' << "Press the Enter key to continue.";
				} while (std::cin.get() != '\n');
			}
		}
	}

}


// Ultra fast anagram generator

#include <algorithm>
#include <bitset>
#include <cassert>
#include <codecvt>
#include <fstream>
#include <ios>
#include <iostream>
#include <limits>
#include <locale>
#include <numeric>
#include <optional>
#include <sstream>
#include <string>
#include <tuple>
#include <unordered_map>
#include <utility>
#include <vector>

#include <boost/functional/hash.hpp>
#include <boost/program_options.hpp>

#include <unicode/stringoptions.h>
#include <unicode/uchar.h>
#include <unicode/unistr.h>
#include <unicode/ustream.h>
#include <unicode/utypes.h>

using std::bitset;
using std::cerr;
using std::cout;
using std::endl;
using std::forward;
using std::ifstream;
using std::ios;
using std::nullopt;
using std::optional;
using std::ostream;
using std::ostringstream;
using std::pair;
using std::string;
using std::tie;
using std::unordered_map;
using std::vector;

using boost::hash_range;
using icu::UnicodeString;

typedef unsigned __int128 bigint;

namespace po = boost::program_options;

// total maximum of letters in input
constexpr int MAX_LETTERS = 128;

typedef uint8_t CharIdx;
constexpr int MAX_CHARIDX = std::numeric_limits<CharIdx>::max();

// first 168 primes
constexpr int PRIMES[] = {
    2, 3, 5, 7, 11, 13, 17, 19, 23, 29, 31, 37, 41, 43, 47, 53, 59, 61, 67, 71,
    73, 79, 83, 89, 97, 101, 103, 107, 109, 113, 127, 131, 137, 139, 149, 151,
    157, 163, 167, 173, 179, 181, 191, 193, 197, 199, 211, 223, 227, 229, 233,
    239, 241, 251, 257, 263, 269, 271, 277, 281, 283, 293, 307, 311, 313, 317,
    331, 337, 347, 349, 353, 359, 367, 373, 379, 383, 389, 397, 401, 409, 419,
    421, 431, 433, 439, 443, 449, 457, 461, 463, 467, 479, 487, 491, 499, 503,
    509, 521, 523, 541, 547, 557, 563, 569, 571, 577, 587, 593, 599, 601, 607,
    613, 617, 619, 631, 641, 643, 647, 653, 659, 661, 673, 677, 683, 691, 701,
    709, 719, 727, 733, 739, 743, 751, 757, 761, 769, 773, 787, 797, 809, 811,
    821, 823, 827, 829, 839, 853, 857, 859, 863, 877, 881, 883, 887, 907, 911,
    919, 929, 937, 941, 947, 953, 967, 971, 977, 983, 991, 997
};

// If the given function returns false, terminate.
template <typename Fn>
bool forAllAlpha(const UnicodeString &s, Fn &&f) {
    for (int j = 0, je = s.length(); j<je; j++) {
        UChar c = s[j];
        if (u_isUAlphabetic(c)) {
            if (!f(c))
                return false;
        }
    }
    return true;
}

// We map the characters in the input to integers starting from 0 (and
// that must fit in CharIdx).
typedef unordered_map<UChar, int> CharMap;

static size_t hash_bigint(const bigint &m) {
    // Let's decide the lowest bits are good enough, even though it's
    // guaranteed we have count(most_common_letter) trailing zero
    // bits.
    return static_cast<size_t>(m);
}

// A multiset of characters.
//
// The allowable characters are passed in the CharMap, which maps
// unicode characters to integers starting from 0.
//
// To facilitate the required multiset operations, namely subtraction
// and subset testing, we map a multiset to an integer (which may not
// fit in a fixed size integer type)
//
//    product(nth_prime(i)**char_count(i)).
//
// Now,
//
// * x is a subset of y iff x.num % y.num == 0
// * x-y maps to integer division.
//
// The order of characters is chosen such that the most common
// characters in the input map to smallest primes, which results in
// smallest numbers.
class CharBag {
public:
    static optional<CharBag> fromUString(const UnicodeString &str, const CharMap &charmap);
    static optional<CharBag> fromLowerUString(const UnicodeString &str, const CharMap &charmap);
    static optional<CharBag> fromNativeString(const string &str, const CharMap &charmap) {
        return fromUString(UnicodeString(str.c_str()), charmap);
    }
    const bigint &num() const { return m_num; }
    const int size() const { return m_size; }
    size_t hash() const { return m_hash; }

    bool empty() const { return m_size == 0; }

    bool isSubsetOf(const CharBag &other) const {
        auto cs_opt = other-*this;
        return (bool)cs_opt;
    }

    optional<CharBag> operator-(const CharBag &rhs) const;
    bool operator==(const CharBag &rhs) const;
private:
    CharBag(bigint num, int size) :
        m_num(num), m_size(size), m_hash(compute_hash(m_size, m_num)) {}

    static size_t compute_hash(const int &size, const bigint &num) {
        // As a hack to find words faster, we want longer words to map
        // to smaller hashes. Do this by mapping negated size to the
        // high bits of the hash.
        constexpr int bits = std::numeric_limits<size_t>::digits;
        size_t hi = (~static_cast<size_t>(size)) << (bits-7);
        size_t lo = hash_bigint(num) & ((size_t(1) << (bits-7)) - 1);
        return hi | lo;
    }

    bigint m_num;
    int m_size;
    size_t m_hash;
};

namespace std {
  template <> struct hash<CharBag> {
    size_t operator()(const CharBag &cs) const {
        return cs.hash();
    }
  };
}

bool CharBag::operator==(const CharBag &rhs) const {
    return m_hash == rhs.m_hash && m_num == rhs.m_num;
}

optional<CharBag> CharBag::operator-(const CharBag &rhs) const {
    if (rhs.m_size > m_size)
        return nullopt;

    if (m_size == rhs.m_size && m_num == rhs.m_num)
        return CharBag{bigint(1), 0};

    // if (!mpz_divisible_p(m_num.get_mpz_t(), rhs.m_num.get_mpz_t()))
    //  return nullopt;
    if (m_num % rhs.m_num != 0)
        return nullopt;

    return CharBag{m_num/rhs.m_num, m_size-rhs.m_size};
}

optional<CharBag> CharBag::fromLowerUString(const UnicodeString &str, const CharMap &charmap) {
    bigint n(1);
    int size = 0;

    static_assert(sizeof(PRIMES)/sizeof(PRIMES[0]) >= MAX_LETTERS);

    if (forAllAlpha(str, [&n, &size, &charmap](UChar c) {
            auto it = charmap.find(c);
            if (it == charmap.end())
                return false;
            int idx = it->second;
            assert(idx < MAX_LETTERS);
            n *= PRIMES[idx];
            ++size;
            return true;
            })) {
        return CharBag(n, size);
    } else
        return nullopt;
}

optional<CharBag> CharBag::fromUString(const UnicodeString &str_, const CharMap &charmap) {
    UnicodeString str(str_);
    str.toLower();
    return fromLowerUString(str, charmap);
}

// [[maybe_unused]]
// static ostream &operator<<(ostream &os, const CharBag &cs) {
//     return os << "CharBag{" << cs.size() << ", " << cs.num() << "}";
// }

// [[maybe_unused]]
// static ostream &operator<<(ostream &os, const optional<CharBag> &cs) {
//     if (cs)
//      os << *cs;
//     else
//      os << "nil";
//     return os;
// }

// from https://stackoverflow.com/questions/17074324/
template <typename T>
void apply_permutation_in_place(std::vector<T>& vec,
                                const std::vector<std::size_t>& p) {
    std::vector<bool> done(vec.size());
    for (std::size_t i = 0; i < vec.size(); ++i) {
        if (done[i])
            continue;
        done[i] = true;
        std::size_t prev_j = i;
        std::size_t j = p[i];
        while (i != j) {
            std::swap(vec[prev_j], vec[j]);
            done[j] = true;
            prev_j = j;
            j = p[j];
        }
    }
}

static vector<char> slurp(const string &fileName) {
    ifstream ifs(fileName.c_str(), ios::binary | ios::ate);

    ifstream::pos_type size = ifs.tellg();
    ifs.seekg(0, ios::beg);

    vector<char> bytes(size);
    ifs.read(bytes.data(), size);

    return bytes;
}

// The dictionary words are sorted by length (number of alphabetic
// characters). This is important, it's used in
// forAllAnagrams_iter_last().
static pair<vector<vector<string>>, vector<CharBag>> loadDictionary(
    const string &fname, const CharBag &cset, const CharMap &cmap) {
    const auto raw_contents = slurp(fname);
    UnicodeString contents = UnicodeString(raw_contents.data(), raw_contents.size());
    contents.toLower();
    size_t contents_len = contents.length();

    vector<vector<string>> words;
    vector<CharBag> charbags;
    unordered_map<CharBag, size_t> charbag_map;

    size_t count = 0;

    size_t line_start = 0;
    while (true) {
        auto newline = contents.indexOf(UChar('\n'), line_start);

        size_t endpos;
        if (newline == -1)
            endpos = contents_len;
        else
            endpos = newline;

        auto str_len = endpos - line_start;
        auto line = contents.tempSubString(line_start, str_len);
        if (str_len > 0) {
            optional<CharBag> cs = CharBag::fromLowerUString(line, cmap);
            if (cs && cset - *cs) {
                if (cs->empty())
                    continue;
                ++count;

                ostringstream line_sstream;
                line_sstream << line;

                auto it = charbag_map.find(*cs);
                if (it == charbag_map.end()) {
                    words.emplace_back(vector<string>{{line_sstream.str()}});
                    charbags.emplace_back(*cs);
                    charbag_map[*cs] = words.size()-1;
                } else
                    words[it->second].emplace_back(line_sstream.str());
            }
        }
        if (newline == -1)
            break;
        else
            line_start = newline+1;
    }

    // Sort the words by charbag hash.

    vector<size_t> hash_sort_order(charbags.size());
    std::iota(hash_sort_order.begin(), hash_sort_order.end(), 0);

    std::sort(hash_sort_order.begin(), hash_sort_order.end(),
              [&charbags](int a, int b) { return charbags[a].hash() < charbags[b].hash(); });

    apply_permutation_in_place(words, hash_sort_order);
    apply_permutation_in_place(charbags, hash_sort_order);

    //cerr << "Loaded " << count << " dictionary words, " << words.size() << " distinct." << endl;
    return {words, charbags};
}

template <typename Fn>
void forAllAnagrams_iter_last(const vector<CharBag> &dict_charbags,
                              const vector<int> &possible_charbags,
                              const CharBag &charbag, Fn &&f,
                              vector<size_t> &words, size_t start_idx) {
    size_t required_hash = charbag.hash();

    auto compare_charbags = [&dict_charbags, required_hash](const int a, const int b) {
        size_t a_hash, b_hash;

        // FIXME how to do this properly? This is really ugly.
        if (a == -1)
            a_hash = required_hash;
        else
            a_hash = dict_charbags[a].hash();
        if (b == -1)
            b_hash = required_hash;
        else
            b_hash = dict_charbags[b].hash();

        return a_hash < b_hash;
    };

    {
        auto it = std::lower_bound(possible_charbags.begin() + start_idx,
                                   possible_charbags.end(), -1, compare_charbags);

        for (auto ie = possible_charbags.end();
             it != ie && dict_charbags[*it].hash() == required_hash; ++it)
            if (charbag == dict_charbags[*it]) {
                words.emplace_back(*it);
                f(words);
                words.pop_back();
                // There's at most one hit, and we found one.
                return;
            }
    }
}

template <typename Fn>
void forAllAnagrams_iter(const vector<CharBag> &dict_charbags,
                         const vector<int> &old_possible_charbags,
                         const CharBag &charbag, Fn &&f,
                         vector<size_t> &words, size_t start_idx, int curr_len, int max_len) {
    if (curr_len+1 >= max_len) {
        forAllAnagrams_iter_last(dict_charbags, old_possible_charbags, charbag, f,
                                 words, start_idx);
        return;
    }
    vector<int> possible_charbags;
    for (int i = start_idx, ie = old_possible_charbags.size(); i<ie; i++)
        if (dict_charbags[old_possible_charbags[i]].isSubsetOf(charbag))
            possible_charbags.emplace_back(old_possible_charbags[i]);

    for (int i = 0, ie = possible_charbags.size(); i<ie; i++) {
        auto cs = (charbag - dict_charbags[possible_charbags[i]]).value();
        words.emplace_back(possible_charbags[i]);
        if (cs.empty())
            f(words);
        else
            forAllAnagrams_iter(dict_charbags, possible_charbags,
                                cs, forward<Fn>(f), words, i, curr_len+1, max_len);
        words.pop_back();
    }
}

template <typename Fn>
void forAllAnagrams(const vector<CharBag> &dict_charbags, const CharBag &charbag,
                    int max_len, Fn &&f) {
    vector<size_t> words;
    vector<int> possible_charbags(dict_charbags.size());
    std::iota(possible_charbags.begin(), possible_charbags.end(), 0);
    forAllAnagrams_iter(dict_charbags, possible_charbags,
                        charbag, forward<Fn>(f), words, 0, 0, max_len);
}

// The words vector contains vectors of anagram-equivalent words.
// Output all possible combinations of them.
static void outputWords(ostream &stream, const vector<size_t> &word_idxs,
                        const vector<vector<string>> &words) {
    int size = word_idxs.size();
    vector<size_t> idxs(size);

    while (true) {
        stream << words[word_idxs[0]][idxs[0]];
        for (int i = 1; i<size; i++)
            stream << " " << words[word_idxs[i]][idxs[i]];
        stream << endl;

        // increment
        int curr = size-1;
        while (curr >= 0 && ++idxs[curr] == words[word_idxs[curr]].size()) {
            idxs[curr] = 0;
            curr--;
        }
        if (curr < 0)
            break;
    }
}

static void usage(char * const *argv, po::options_description &visible) {
    cout << "Usage: " << argv[0] << " [options] sentence" << endl;
    cout << visible << endl;
    exit(0);
}

static po::variables_map parse_args(int argc, char * const *argv) {
    bool help = false;

    po::options_description visible("Allowed options"), cmdline_opt;
    visible.add_options()
        ("help,h", po::bool_switch(&help)->default_value(false), "show this help")
        ("dict,d", po::value<string>()->default_value("words.txt"),
         "dictionary (word list) to use")
        ("len,l", po::value<int>()->default_value(3), "maximum anagram length in words")
        ("remove,r", po::value<string>()->default_value(""),
         "remove characters from input (find anagrams containing given string)");

    po::options_description hidden;
    hidden.add_options()
        ("sentence", po::value<string>()->required(), "sentence");

    cmdline_opt.add(visible).add(hidden);

    po::positional_options_description p;
    p.add("sentence", 1);

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).options(cmdline_opt).positional(p).run(), vm);
        po::notify(vm);
    } catch (po::error &) {
        help = true;
    }

    if (help)
        usage(argv, visible);

    return vm;
}

CharMap generateCharMap(const UnicodeString &input) {
    CharMap charmap;

    unordered_map<UChar, int> char_counts;

    forAllAlpha(input, [&char_counts](UChar c) {
            ++char_counts[c];
            return true;
        });

    if (char_counts.size() > MAX_LETTERS) {
        cerr << "Error: More than " << MAX_CHARIDX+1
             << " different characters in input." << endl;
        exit(1);
    }

    vector<pair<int, UChar>> charmap_with_counts;
    for (const auto &it : char_counts)
        charmap_with_counts.emplace_back(-it.second, it.first);

    std::sort(charmap_with_counts.begin(), charmap_with_counts.end());

    int i = 0;
    for (const auto &p : charmap_with_counts)
        charmap[p.second] = i++;

    return charmap;
}

int main(int argc, char **argv) {
    std::locale::global(std::locale(""));

    auto vm = parse_args(argc, argv);

    UnicodeString input = UnicodeString(vm["sentence"].as<string>().c_str()).toLower();
    UnicodeString remove = UnicodeString(vm["remove"].as<string>().c_str()).toLower();

    CharMap charmap;
    vector<UChar> reverse_charmap;

    charmap = generateCharMap(input);

    auto input_charbag = CharBag::fromUString(input, charmap).value();
    auto remove_charbag_opt = CharBag::fromUString(remove, charmap);

    if (remove_charbag_opt == nullopt || !remove_charbag_opt.value().isSubsetOf(input_charbag)) {
        cerr << "Error: characters for substring \"" << remove
                << "\" not found in string \"" << input
                << "\"!" << endl;
        exit(2);
    }

    auto stripped_input_charbag = (input_charbag - remove_charbag_opt.value()).value();

    vector<vector<string>> dict_words;
    vector<CharBag> dict_charbags;
    tie(dict_words, dict_charbags) = loadDictionary(
        vm["dict"].as<string>(), stripped_input_charbag, charmap);

    forAllAnagrams(dict_charbags, stripped_input_charbag, vm["len"].as<int>(),
                   [&dict_words](const vector<size_t> &word_idxs) {
            assert(!word_idxs.empty());
            outputWords(cout, word_idxs, dict_words);
        });
}


/**
* This file is part of Fast-Planner.
*
* Copyright 2019 Boyu Zhou, Aerial Robotics Group, Hong Kong University of Science and Technology, <uav.ust.hk>
* Developed by Boyu Zhou <bzhouai at connect dot ust dot hk>, <uv dot boyuzhou at gmail dot com>
* for more information see <https://github.com/HKUST-Aerial-Robotics/Fast-Planner>.
* If you use this code, please cite the respective publications as
* listed on the above website.
*
* Fast-Planner is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Fast-Planner is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public License
* along with Fast-Planner. If not, see <http://www.gnu.org/licenses/>.
*/

#ifndef _LINEAR_OBJ_MODEL_H_
#define _LINEAR_OBJ_MODEL_H_

#include <Eigen/Eigen>
#include <algorithm>
#include <iostream>

class LinearObjModel {
private:
  /* data */
public:
  LinearObjModel(/* args */);
  ~LinearObjModel();

  void initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw, double yaw_dot,
                  Eigen::Vector3d color, Eigen::Vector3d scale);

  void setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc);

  void update(double dt);  // linear trippler integrator model

  static bool collide(LinearObjModel& obj1, LinearObjModel& obj2);

  // void setInput(Eigen::Vector3d acc) { acc_ = acc; }
  void setInput(Eigen::Vector3d vel) {
    vel_ = vel;
  }

  void setYawDot(double yaw_dot) {
    yaw_dot_ = yaw_dot;
  }

  Eigen::Vector3d getPosition() {
    return pos_;
  }
  void setPosition(Eigen::Vector3d pos) {
    pos_ = pos;
  }

  Eigen::Vector3d getVelocity() {
    return vel_;
  }

  void setVelocity(double x, double y, double z) {
    vel_ = Eigen::Vector3d(x, y, z);
  }

  Eigen::Vector3d getColor() {
    return color_;
  }
  Eigen::Vector3d getScale() {
    return scale_;
  }

  double getYaw() {
    return yaw_;
  }

private:
  Eigen::Vector3d pos_, vel_, acc_;
  Eigen::Vector3d color_, scale_;
  double yaw_, yaw_dot_;

  Eigen::Vector3d bound_;
  Eigen::Vector2d limit_v_, limit_a_;
};

LinearObjModel::LinearObjModel(/* args */) {
}

LinearObjModel::~LinearObjModel() {
}

void LinearObjModel::initialize(Eigen::Vector3d p, Eigen::Vector3d v, Eigen::Vector3d a, double yaw,
                                double yaw_dot, Eigen::Vector3d color, Eigen::Vector3d scale) {
  pos_ = p;
  vel_ = v;
  acc_ = a;
  color_ = color;
  scale_ = scale;

  yaw_ = yaw;
  yaw_dot_ = yaw_dot;
}

void LinearObjModel::setLimits(Eigen::Vector3d bound, Eigen::Vector2d vel, Eigen::Vector2d acc) {
  bound_ = bound;
  limit_v_ = vel;
  limit_a_ = acc;
}

void LinearObjModel::update(double dt) {
  Eigen::Vector3d p0, v0, a0;
  p0 = pos_, v0 = vel_, a0 = acc_;

  /* ---------- use acc as input ---------- */
  // vel_ = v0 + acc_ * dt;
  // for (int i = 0; i < 3; ++i)
  // {
  //   if (vel_(i) > 0) vel_(i) = std::max(limit_v_(0), std::min(vel_(i),
  //   limit_v_(1)));
  //   if (vel_(i) <= 0) vel_(i) = std::max(-limit_v_(1), std::min(vel_(i),
  //   -limit_v_(0)));
  // }

  // pos_ = p0 + v0 * dt + 0.5 * acc_ * pow(dt, 2);
  // for (int i = 0; i < 2; ++i)
  // {
  //   pos_(i) = std::min(pos_(i), bound_(i));
  //   pos_(i) = std::max(pos_(i), -bound_(i));
  // }
  // pos_(2) = std::min(pos_(2), bound_(2));
  // pos_(2) = std::max(pos_(2), 0.0);

  /* ---------- use vel as input ---------- */
  pos_ = p0 + v0 * dt;
  for (int i = 0; i < 2; ++i) {
    pos_(i) = std::min(pos_(i), bound_(i));
    pos_(i) = std::max(pos_(i), -bound_(i));
  }
  pos_(2) = std::min(pos_(2), bound_(2));
  pos_(2) = std::max(pos_(2), 0.0);

  yaw_ += yaw_dot_ * dt;

  const double PI = 3.1415926;
  if (yaw_ > 2 * PI) yaw_ -= 2 * PI;

  /* ---------- reflect when collide with bound ---------- */
  const double tol = 0.1;
  if (pos_(0) > bound_(0) - tol) {
    pos_(0) = bound_(0) - tol;
    vel_(0) = -vel_(0);
  }
  if (pos_(0) < -bound_(0) + tol) {
    pos_(0) = -bound_(0) + tol;
    vel_(0) = -vel_(0);
  }

  if (pos_(1) > bound_(1) - tol) {
    pos_(1) = bound_(1) - tol;
    vel_(1) = -vel_(1);
  }
  if (pos_(1) < -bound_(1) + tol) {
    pos_(1) = -bound_(1) + tol;
    vel_(1) = -vel_(1);
  }

  if (pos_(2) > bound_(2) - tol) {
    pos_(2) = bound_(2) - tol;
    vel_(2) = -vel_(2);
  }
  if (pos_(2) < tol) {
    pos_(2) = tol;
    vel_(2) = -vel_(2);
  }
}

bool LinearObjModel::collide(LinearObjModel& obj1, LinearObjModel& obj2) {
  Eigen::Vector3d pos1, pos2, vel1, vel2, scale1, scale2;
  pos1 = obj1.getPosition();
  vel1 = obj1.getVelocity();
  scale1 = obj1.getScale();

  pos2 = obj2.getPosition();
  vel2 = obj2.getVelocity();
  scale2 = obj2.getScale();

  /* ---------- collide ---------- */
  bool collide = fabs(pos1(0) - pos2(0)) < 0.5 * (scale1(0) + scale2(0)) &&
      fabs(pos1(1) - pos2(1)) < 0.5 * (scale1(1) + scale2(1)) &&
      fabs(pos1(2) - pos2(2)) < 0.5 * (scale1(2) + scale2(2));

  if (collide) {
    double tol[3];
    tol[0] = 0.5 * (scale1(0) + scale2(0)) - fabs(pos1(0) - pos2(0));
    tol[1] = 0.5 * (scale1(1) + scale2(1)) - fabs(pos1(1) - pos2(1));
    tol[2] = 0.5 * (scale1(2) + scale2(2)) - fabs(pos1(2) - pos2(2));

    for (int i = 0; i < 3; ++i) {
      if (tol[i] < tol[(i + 1) % 3] && tol[i] < tol[(i + 2) % 3]) {
        vel1(i) = -vel1(i);
        vel2(i) = -vel2(i);
        obj1.setVelocity(vel1(0), vel1(1), vel1(2));
        obj2.setVelocity(vel2(0), vel2(1), vel2(2));

        if (pos1(i) >= pos2(i)) {
          pos1(i) += tol[i];
          pos2(i) -= tol[i];
        } else {
          pos1(i) -= tol[i];
          pos2(i) += tol[i];
        }
        obj1.setPosition(pos1);
        obj2.setPosition(pos2);

        break;
      }
    }

    return true;
  } else {
    return false;
  }
}

#endif


/*
 * test_data_octomap.cpp
 *
 *  Created on: 23 Aug 2012
 *      Author: andreas
 */

#include <ros/ros.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/ply_io.h>
#include <octomap/octomap.h>
#include <boost/algorithm/string/replace.hpp>
#include <iostream>
#include <fstream>
#include <boost/filesystem.hpp>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH

using std::string;
using boost::algorithm::replace_first;

string meshlabscript_filename;
string map_path;
string model_path;
double target_resolution = 0.05;
bool filter_z = true;

double map_resolution = 0.05;

namespace fs = boost::filesystem;

void supersample(pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud, boost::shared_ptr<octomap::OcTree> ocTree, const octomap::OcTree::iterator& it)
{
    double half_resolution = map_resolution / 2.0;
    double quarter_resolution = half_resolution / 2.0;
    for (int x = 0; x < 2; x++)
    {
        for (int y = 0; y < 2; y++)
        {
            for (int z = 0; z < 2; z++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - quarter_resolution + x * half_resolution;
                point.y = it.getY() - quarter_resolution + y * half_resolution;
                point.z = it.getZ() - quarter_resolution + z * half_resolution;
            }
        }
    }
}

bool isNodeOccupied(const boost::shared_ptr<octomap::OcTree> ocTree, octomap::OcTreeKey key, unsigned int depth)
{
    const octomap::OcTreeNode* node = ocTree->search(key, depth);
    if (node != NULL)
    {
        return ocTree->isNodeOccupied(node);
    }
    return false;
}

void extract_surface(pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud, const boost::shared_ptr<octomap::OcTree> ocTree, const octomap::OcTree::iterator& it)
{
    unsigned int depth = it.getDepth();
    double cube_size = map_resolution / 2.0;
    double half_cube_size = cube_size / 2.0;
    // check faces neighbour
    octomap::point3d point = it.getCoordinate();
    point += octomap::point3d(map_resolution, 0, 0);
    octomap::OcTreeKey neighbour_key = ocTree->coordToKey(point, depth);
    bool inner_node = true;
    if (! isNodeOccupied(ocTree, neighbour_key, depth))
    {
        for (int y = 0; y < 2; y++)
        {
            for (int z = 0; z < 2; z++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - half_cube_size + cube_size;
                point.y = it.getY() - half_cube_size + y * cube_size;
                point.z = it.getZ() - half_cube_size + z * cube_size;
            }
        }
    }
    point = it.getCoordinate();
    point += octomap::point3d(-map_resolution, 0, 0);
    neighbour_key = ocTree->coordToKey(point, depth);
    if (! isNodeOccupied(ocTree, neighbour_key, depth))
    {
        for (int y = 0; y < 2; y++)
        {
            for (int z = 0; z < 2; z++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - half_cube_size;
                point.y = it.getY() - half_cube_size + y * cube_size;
                point.z = it.getZ() - half_cube_size + z * cube_size;
            }
        }
    }
    point = it.getCoordinate();
    point += octomap::point3d(0, map_resolution, 0);
    neighbour_key = ocTree->coordToKey(point, depth);
    if (! isNodeOccupied(ocTree, neighbour_key, depth))
    {
        string model_path;

        for (int x = 0; x < 2; x++)
        {
            for (int z = 0; z < 2; z++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - half_cube_size + x * cube_size;
                point.y = it.getY() - half_cube_size + cube_size;
                point.z = it.getZ() - half_cube_size + z * cube_size;
            }
        }
    }
    point = it.getCoordinate();
    point += octomap::point3d(0, -map_resolution, 0);
    neighbour_key = ocTree->coordToKey(point, depth);
    if (! isNodeOccupied(ocTree, neighbour_key, depth))
    {
        for (int x = 0; x < 2; x++)
        {
            for (int z = 0; z < 2; z++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - half_cube_size + x * cube_size;
                point.y = it.getY() - half_cube_size;
                point.z = it.getZ() - half_cube_size + z * cube_size;
            }
        }
    }
    point = it.getCoordinate();
    point += octomap::point3d(0, 0, map_resolution);
    neighbour_key = ocTree->coordToKey(point, depth);
    if (! isNodeOccupied(ocTree, neighbour_key, depth))
    {
        for (int x = 0; x < 2; x++)
        {
            for (int y = 0; y < 2; y++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - half_cube_size + x * cube_size;
                point.y = it.getY() - half_cube_size + y * cube_size;
                point.z = it.getZ() - half_cube_size + cube_size;
            }
        }
    }
    point = it.getCoordinate();
    point += octomap::point3d(0, 0, -map_resolution);
    neighbour_key = ocTree->coordToKey(point, depth);
    if (! isNodeOccupied(ocTree, neighbour_key, depth))
    {
        for (int x = 0; x < 2; x++)
        {
            for (int y = 0; y < 2; y++)
            {
                pclCloud->push_back(pcl::PointXYZ());
                pcl::PointXYZ& point = pclCloud->back();
                point.x = it.getX() - half_cube_size + x * cube_size;
                point.y = it.getY() - half_cube_size + y * cube_size;
                point.z = it.getZ() - half_cube_size;
            }
        }
    }
}

pcl::PointCloud<pcl::PointXYZ>::Ptr octomap_to_pointcloud(boost::shared_ptr<octomap::OcTree> ocTree, double target_resolution = 0.1)
{
    ocTree->expand();
    map_resolution = ocTree->getResolution();
    int max_depth = 16;
    for (; max_depth > 1 && map_resolution < target_resolution; max_depth--)
    {
        map_resolution *= 2;
    }
    ROS_INFO_STREAM("resolution: map: "<<ocTree->getResolution()<<" target: "<<target_resolution<<" corrected: "<<map_resolution<<" max_depth: "<< max_depth);

    // init pointcloud:
    pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud(new pcl::PointCloud<pcl::PointXYZ>);
    for (octomap::OcTree::iterator it = ocTree->begin(max_depth); it != ocTree->end(); it++)
    {
        if (ocTree->isNodeOccupied(*it))
        {
            if (filter_z && (it.getZ() < 0.0 || it.getZ() > 2.0))
            {
                continue;
            }
//            supersample(pclCloud, ocTree, it);
            extract_surface(pclCloud, ocTree, it);
        }
    }
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud (pclCloud);
    sor.setLeafSize (0.01f, 0.01f, 0.01f);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
    sor.filter (*cloud_filtered);
    ROS_INFO("octomap converted to point cloud.");
    return cloud_filtered;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "octomap_sdf");
    ros::NodeHandle nh;

    // Initialize ROS
    ROS_ASSERT(argc == 2);
    fs::path package(argv[1]);
    ros::param::get("~script_filename", meshlabscript_filename);
    ros::param::get("~map_path", map_path);
    ros::param::get("~model_path", model_path);
    ros::param::get("~target_resolution", target_resolution);
    ros::param::get("~filter_z", filter_z);

    fs::path map_abs = package / fs::path(map_path);
    ROS_ASSERT_MSG(fs::exists(map_abs), ("octomap file not found: "+map_abs.string()).c_str());
    fs::path model_abs = package / fs::path(model_path);
    fs::create_directories(model_abs.parent_path());

    ROS_INFO("load_octomap");
    boost::shared_ptr<octomap::OcTree> ocTree(new octomap::OcTree(0.05));
    ocTree->readBinary(map_abs.string());

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = octomap_to_pointcloud(ocTree, target_resolution);
    pcl::io::savePLYFile("/tmp/cloud.ply", *cloud, true);
    string meshlab_command = "meshlabserver -s [scrip_file] -i /tmp/cloud.ply -o [model_path]";
    replace_first(meshlab_command, "[scrip_file]", meshlabscript_filename);
    replace_first(meshlab_command, "[model_path]", model_abs.string());
    int value = system(meshlab_command.c_str());
    ROS_ASSERT(value == 0);
    ROS_INFO("octomap conversion finished.");

    return 0;
}


// Copyright (c) 2014 The Bitcoin Core developers
// Copyright (c) 2014-2015 The Dash developers
// Copyright (c) 2015-2017 The PIVX developers
// Copyright (c) 2017-2018 The ROBI developers
// Distributed under the MIT/X11 software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "primitives/transaction.h"
#include "main.h"

#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(main_tests)

CAmount nMoneySupplyPoWEnd = 100000000 * COIN;

BOOST_AUTO_TEST_CASE(subsidy_limit_test)
{
    CAmount nSum = 0;
    for (int nHeight = 0; nHeight < 1; nHeight += 1) {
        /* premine in block 1 */
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 3000000 * COIN);
        nSum += nSubsidy;
    }

    for (int nHeight = 1; nHeight < 500; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 5 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 500; nHeight < 10000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 50 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 10000; nHeight < 15000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 90 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 15000; nHeight < 25000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 150 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 25000; nHeight < 75000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 70 * COIN);
        nSum += nSubsidy;
    }
    for (int nHeight = 75000; nHeight < 110000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 50 * COIN);
        nSum += nSubsidy;
	}
    for (int nHeight = 110000; nHeight < 160000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 25 * COIN);
        nSum += nSubsidy;
	}
    for (int nHeight = 160000; nHeight < 200000; nHeight += 1) {
        CAmount nSubsidy = GetBlockValue(nHeight);
        BOOST_CHECK(nSubsidy <= 10 * COIN);
        nSum += nSubsidy;
    }

	BOOST_CHECK(nSum > 0 && nSum <= nMoneySupplyPoWEnd);
}

BOOST_AUTO_TEST_SUITE_END()


#include "Generic/common/leak_detection.h"

#include "OneSidedUnlabeledDisagreementLoss.h"
#include <iostream>
#include <limits>
#include <boost/lexical_cast.hpp>
#include "Generic/common/SessionLogger.h"
#include "Generic/common/UnrecoverableException.h"
#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include "LearnIt/Eigen/Sparse"
#include "ActiveLearning/DataView.h"
#include "ActiveLearning/ActiveLearningData.h"

using Eigen::VectorXd;
using Eigen::SparseVector;

OneSidedUnlabeledDisagreementLoss::OneSidedUnlabeledDisagreementLoss(
	InferenceDataView_ptr optData, InferenceDataView_ptr referenceData,
	ActiveLearningData_ptr alData, double whenAnnotatedLossWeight,
	double whenUnannotatedLossWeight)
: _optimizeData(optData), _referenceData(referenceData), _alData(alData),
_whenAnnotatedLossWeight(whenAnnotatedLossWeight),
_whenUnannotatedLossWeight(whenUnannotatedLossWeight)
{}

double OneSidedUnlabeledDisagreementLoss::operator()(VectorXd& gradient,
		size_t thread_idx, size_t n_threads) const
{
	double loss = 0.0;
	for (unsigned int i=thread_idx; i<_optimizeData->nInstances(); i+=n_threads) {
		if (!_alData->instanceAnnotation(i)) {
				// 1= slot
				// 2= sent
			SparseVector<double>::InnerIterator optIt(_optimizeData->features(i));
			SparseVector<double>::InnerIterator refIt(_referenceData->features(i));

			// skip instances where one FV is empty
			if (optIt && refIt) {
				// generally, we want to penalize disagreement more when
				// we have an annotation to make us more sure of one of
				// the models
				double lossWeight = _alData->instanceHasAnnotatedFeature(i)
					? _whenAnnotatedLossWeight : _whenUnannotatedLossWeight;

				double posOpt = _optimizeData->prediction(i);
				double negOpt = 1.0 - posOpt;
				double posRef= _referenceData->prediction(i);
				double negRef = 1.0 - posRef;

				loss -= lossWeight * (posRef*(log(posRef)-log(posOpt))
					+ negRef*(log(negRef)-log(negOpt)));

				double factor = -lossWeight * (posOpt - posRef);

				for (; optIt; ++optIt) {
					gradient(optIt.index()) += factor * optIt.value();
				}
			}
		}
	}
	_debug_loss = loss;
	return loss;
}

std::wstring OneSidedUnlabeledDisagreementLoss::status() const {
	return L"diss_loss = " + boost::lexical_cast<std::wstring>(_debug_loss);
}

void OneSidedUnlabeledDisagreementLoss::snapshot() {
}

double OneSidedUnlabeledDisagreementLoss::forInstance(int inst,
													  Eigen::SparseVector<double>& gradient) const
{
	double ret = 0.0;
	int i = inst;
	if (!_alData->instanceAnnotation(i)) {
		// 1= slot
		// 2= sent
		SparseVector<double>::InnerIterator optIt(_optimizeData->features(i));
		SparseVector<double>::InnerIterator refIt(_referenceData->features(i));

		// skip instances where one FV is empty
		if (optIt && refIt) {
			// generally, we want to penalize disagreement more when
			// we have an annotation to make us more sure of one of
			// the models
			double lossWeight = _alData->instanceHasAnnotatedFeature(i)
				? _whenAnnotatedLossWeight : _whenUnannotatedLossWeight;

			double posOpt = _optimizeData->prediction(i);
			double negOpt = 1.0 - posOpt;
			double posRef= _referenceData->prediction(i);
			double negRef = 1.0 - posRef;

			ret = -lossWeight * (posRef*(log(posRef)-log(posOpt))
				+ negRef*(log(negRef)-log(negOpt)));

			double factor = -lossWeight * (posOpt - posRef);

			for (; optIt; ++optIt) {
				gradient.coeffRef(optIt.index()) += factor * optIt.value();
			}
		}
	}
	return ret;
}