// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2015 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2015 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2015 Mateusz Loskot, London, UK.
// Copyright (c) 2014-2015 Samuel Debionne, Grenoble, France.

// This file was modified by Oracle on 2015.
// Modifications copyright (c) 2015, Oracle and/or its affiliates.

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

// Use, modification and distribution is subject to 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_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_SEGMENT_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_SEGMENT_HPP

#include <boost/geometry/core/tags.hpp>

#include <boost/geometry/algorithms/detail/envelope/segment.hpp>
#include <boost/geometry/algorithms/detail/expand/box.hpp>
#include <boost/geometry/algorithms/detail/expand/indexed.hpp>
#include <boost/geometry/algorithms/dispatch/expand.hpp>

namespace boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace expand
{

struct segment_on_spheroid
{
    template <typename Box, typename Segment>
    static inline void apply(Box& box, Segment const& segment)
    {
        Box segment_envelope;
        dispatch::envelope<Segment>::apply(segment, segment_envelope);
        box_on_spheroid::apply(box, segment_envelope);
    }
};

}} // namespace detail::expand
#endif // DOXYGEN_NO_DETAIL

#ifndef DOXYGEN_NO_DISPATCH
namespace dispatch
{

template
<
    typename Box, typename Segment,
    typename StrategyLess, typename StrategyGreater,
    typename CSTagOut, typename CSTag
>
struct expand
    <
        Box, Segment, StrategyLess, StrategyGreater,
        box_tag, segment_tag, CSTagOut, CSTag
    > : detail::expand::expand_indexed<StrategyLess, StrategyGreater>
{};

template
<
    typename Box, typename Segment,
    typename StrategyLess, typename StrategyGreater
>
struct expand
    <
        Box, Segment, StrategyLess, StrategyGreater,
        box_tag, segment_tag,
        spherical_equatorial_tag, spherical_equatorial_tag
    > : detail::expand::segment_on_spheroid
{};

} // namespace dispatch
#endif // DOXYGEN_NO_DISPATCH

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_EXPAND_SEGMENT_HPP


#ifndef EFANNA_BASE_INDEX_H_
#define EFANNA_BASE_INDEX_H_

#include "general/params.hpp"
#include "general/distance.hpp"
#include "general/matrix.hpp"
#include <boost/dynamic_bitset.hpp>
#include <fstream>
#include <iostream>
#include <omp.h>
#include <unordered_set>
#include <mutex>
#include "boost/smart_ptr/detail/spinlock.hpp"
#include <memory>

//#define BATCH_SIZE 200

namespace efanna{

  typedef boost::detail::spinlock Lock;
  typedef std::lock_guard<Lock> LockGuard;
  struct Point {
        unsigned id;
        float dist;
        bool flag;
        Point () {}
        Point (unsigned i, float d, bool f = true): id(i), dist(d), flag(f) {
        }
        bool operator < (const Point &n) const{
            return this->dist < n.dist;
        }
    };

  typedef std::vector<Point>  Points;
  static inline unsigned InsertIntoKnn (Point *addr, unsigned K, Point nn) {
        // find the location to insert
        unsigned j;
        unsigned i = K;
        while (i > 0) {
            j = i - 1;
            if (addr[j].dist <= nn.dist) break;
            i = j;
        }
        // check for equal ID
        unsigned l = i;
        while (l > 0) {
            j = l - 1;
            if (addr[j].dist < nn.dist) break;
            if (addr[j].id == nn.id) return K + 1;
            l = j;
        }
        // i <= K-1
        j = K;
        while (j > i) {
            addr[j] = addr[j-1];
            --j;
        }
        addr[i] = nn;
        return i;
    }
  struct Neighbor {
        std::shared_ptr<Lock> lock;
        float radius;
        float radiusM;
        Points pool;
        unsigned L;
        unsigned Range;
        bool found;
        std::vector<unsigned> nn_old;
        std::vector<unsigned> nn_new;
        std::vector<unsigned> rnn_old;
        std::vector<unsigned> rnn_new;

        Neighbor() : lock(std::make_shared<Lock>())
        {
        }

        unsigned insert (unsigned id, float dist) {
            if (dist > radius) return pool.size();
            LockGuard guard(*lock);
            unsigned l = InsertIntoKnn(&pool[0], L, Point(id, dist, true));
            if (l <= L) {
                if (L + 1 < pool.size()) {
                    ++L;
                }
                else {
                    radius = pool[L-1].dist;
                }
            }
            return l;
        }

        template <typename C>
            void join (C callback) const {
                for (unsigned const i: nn_new) {
                    for (unsigned const j: nn_new) {
                        if (i < j) {
                            callback(i, j);
                        }
                    }
                    for (unsigned j: nn_old) {
                        callback(i, j);
                    }
                }
            }

      };

  template <typename DataType>
  class InitIndex{
  public:
    InitIndex(const Matrix<DataType>& features, const Distance<DataType>* d, const IndexParams& params):
      features_(features),
      distance_(d),
      params_(params)
    {
    }
    virtual ~InitIndex() {};
    virtual void buildTrees(){}

    virtual void buildIndex()
    {
	     buildIndexImpl();
    }
    virtual void buildIndexImpl() = 0;
    virtual void loadIndex(char* filename) = 0;
    virtual void saveIndex(char* filename) = 0;
    virtual void loadTrees(char* filename) = 0;
    virtual void saveTrees(char* filename) = 0;
    virtual void loadGraph(char* filename) = 0;
    virtual void saveGraph(char* filename) = 0;
	virtual void outputVisitBucketNum() = 0;

	const std::vector<std::vector<int> > getResults() const { return nn_results; }

    void saveResults(char* filename){
       std::ofstream out(filename,std::ios::binary);
       std::vector<std::vector<int>>::iterator i;
//std::cout<<nn_results.size()<<std::endl;
       for(i = nn_results.begin(); i!= nn_results.end(); i++){
         std::vector<int>::iterator j;
         int dim = i->size();
//std::cout<<dim<<std::endl;
         out.write((char*)&dim, sizeof(int));
         for(j = i->begin(); j != i->end(); j++){
           int id = *j;
           out.write((char*)&id, sizeof(int));
         }
       }
       out.close();
    }
SearchParams SP;
    void setSearchParams(int epochs, int init_num, int extend_to,int search_trees, int search_lv, int search_method){
      SP.search_epoches = epochs;
      SP.search_init_num = init_num;
      if(extend_to>init_num) SP.extend_to = init_num;
      else  SP.extend_to = extend_to;
      SP.search_depth = search_lv;
      SP.tree_num = search_trees;
      SP.search_method = search_method;
    }

    void nnExpansion_kgraph(size_t K, const DataType* qNow, std::vector<unsigned int>& pool, std::vector<Point>& results){

          unsigned int base_n = features_.get_rows();
          boost::dynamic_bitset<> tbflag(base_n, false);
          boost::dynamic_bitset<> newflag(base_n, false);

          std::vector<Point> knn(K + SP.extend_to +1);

          int remainder = SP.search_init_num % SP.extend_to;
          int nSeg = SP.search_init_num / SP.extend_to;

          //clock_t s,f;

          int Iter = nSeg;
          if (remainder > 0) Iter++;
          int Jter = SP.extend_to;

          for(int i = 0; i<Iter; i++){
        	  if((remainder > 0) && (i == Iter-1))  Jter=remainder;

              unsigned int L = 0;
              for(int j=0; j <Jter ; j++){
                if(!tbflag.test(pool[i*SP.extend_to+j])){
                  knn[L++].id = pool[i*SP.extend_to+j];
                }
              }

              for (unsigned int k = 0; k < L; ++k) {
                  knn[k].dist = distance_->compare(qNow, features_.get_row(knn[k].id), features_.get_cols());
                  newflag.set(knn[k].id);
              }
              std::sort(knn.begin(), knn.begin() + L);

              //s = clock();
              unsigned int k =  0;
              while (k < L) {
                  unsigned int nk = L;
                  if (newflag.test(knn[k].id)){
                    newflag.reset(knn[k].id);
                    tbflag.set(knn[k].id);
                    typename CandidateHeap::reverse_iterator neighbor = knn_graph[knn[k].id].rbegin();
                    for(size_t nnk = 0;nnk < params_.K && neighbor != knn_graph[knn[k].id].rend(); neighbor++, nnk++){
                      if(tbflag.test(neighbor->row_id))continue;
                      tbflag.set(neighbor->row_id);
                      newflag.set(neighbor->row_id);
                      float dist = distance_->compare(qNow, features_.get_row(neighbor->row_id), features_.get_cols());
                      Point nn(neighbor->row_id, dist);
                      unsigned int r = InsertIntoKnn(&knn[0], L, nn);
                   	  if ( (r <= L) && (L + 1 < knn.size())) ++L;
                      if (r < nk) nk = r;
                    }
                  }
                  if (nk <= k) k = nk;
                  else ++k;
              }
              //f = clock();
              //sum = sum + f-s;

              if (L > K) L = K;
              if (results.empty()) {
                  results.reserve(K + 1);
                  results.resize(L + 1);
                  std::copy(knn.begin(), knn.begin() + L, results.begin());
              }
              else {
                  for (unsigned int l = 0; l < L; ++l) {
                      unsigned r = InsertIntoKnn(&results[0], results.size() - 1, knn[l]);
                      if (r < results.size() /* inserted */ && results.size() < (K + 1)) {
                          results.resize(results.size() + 1);
                      }
                  }
              }
          }
          results.pop_back();
    }

    void nnExpansion(size_t K, const DataType* qNow, std::vector<unsigned int>& pool, std::vector<int>& res){

              unsigned int base_n = features_.get_rows();
              boost::dynamic_bitset<> tbflag(base_n, false);
              boost::dynamic_bitset<> newflag(base_n, false);

              CandidateHeap Candidates;

              int remainder = SP.search_init_num % SP.extend_to;
              int nSeg = SP.search_init_num / SP.extend_to;

              int segIter = nSeg;
              if (remainder > 0) segIter++;
              int Jter = SP.extend_to;

              CandidateHeap Results;

              for(int seg = 0; seg<segIter; seg++){
            	  if((remainder > 0) && (seg == segIter-1))  Jter=remainder;

                  for(int j=0; j <Jter ; j++){
                	  unsigned int nn = pool[seg*SP.extend_to+j];
                	  //if(nn>=base_n) std::cout << "query:" << cur << " Init "<< nn << std::endl;
                      if(!tbflag.test(nn)){
                        newflag.set(nn);
                        Candidate<DataType> c(nn, distance_->compare(qNow, features_.get_row(nn), features_.get_cols()));
                        Candidates.insert(c);
                    }
                  }

                  std::vector<unsigned int> ids;
                  int iter=0;
                  while(iter++ < SP.search_epoches){
                      //the heap is max heap
                      ids.clear();
                      typename CandidateHeap::reverse_iterator it = Candidates.rbegin();
                      for(unsigned j = 0; j < SP.extend_to && it != Candidates.rend(); j++,it++){
                    	//  if(it->row_id>=base_n) std::cout<<"query:"<< cur<<" Judge node  "<<it->row_id<<std::endl;
                        if(newflag.test(it->row_id)){
                          newflag.reset(it->row_id);
                          typename CandidateHeap::reverse_iterator neighbor = knn_graph[it->row_id].rbegin();
                          for(; neighbor != knn_graph[it->row_id].rend(); neighbor++){
                        	//  if(neighbor->row_id>=base_n) std::cout<<"query:"<< cur<<" Judge neighbor  "<<neighbor->row_id<<std::endl;
                            if(tbflag.test(neighbor->row_id))continue;
                            tbflag.set(neighbor->row_id);
                            ids.push_back(neighbor->row_id);
                          }
                        }
                      }
                      for(size_t j = 0; j < ids.size(); j++){
                        Candidate<DataType> c(ids[j], distance_->compare(qNow, features_.get_row(ids[j]), features_.get_cols()) );
                        Candidates.insert(c);
                        newflag.set(ids[j]);
                        if(Candidates.size() > (unsigned int)SP.extend_to)Candidates.erase(Candidates.begin());
                      }
                  }
                  typename CandidateHeap::reverse_iterator it = Candidates.rbegin();
                  for(unsigned int j = 0; j < K && it != Candidates.rend(); j++,it++){
                      Results.insert(*it);
                      if(Results.size() > K)Results.erase(Results.begin());
                  }
              }
              typename CandidateHeap::reverse_iterator it = Results.rbegin();
              for(unsigned int j = 0; j < K && it != Candidates.rend(); j++,it++){
            	  res.push_back(it->row_id);
              }
    }

    virtual void knnSearch(int K, const Matrix<DataType>& query){
      getNeighbors(K,query);
    }
    virtual void getNeighbors(size_t K, const Matrix<DataType>& query) = 0;
    virtual void initGraph() = 0;

    //std::vector<unsigned> Range;
    std::vector<std::vector<Point>  > graph;

    std::vector<Neighbor>  nhoods;
    void join(){
      size_t dim = features_.get_cols();
      size_t cc = 0;
#pragma omp parallel for default(shared) schedule(dynamic, 100) reduction(+:cc)
      for(int i = 0; i < nhoods.size(); i++){
        size_t uu = 0;
        nhoods[i].found = false;
/*
        for(size_t newi = 0; newi < nhoods[i].nn_new.size(); newi++){
          for(size_t newj = newi+1; newj < nhoods[i].nn_new.size(); newj++){
            unsigned a = nhoods[i].nn_new[newi];
            unsigned b = nhoods[i].nn_new[newj];
            DataType dist = distance_->compare(
              features_.get_row(a), features_.get_row(b), dim);
            unsigned r = nhoods[a].insert(b,dist);
            if(r < params_.Check_K){uu += 2;}
            nhoods[b].insert(a,dist);
          }

          for(size_t oldj = 0; oldj < nhoods[i].nn_old.size(); oldj++){
            unsigned a = nhoods[i].nn_new[newi];
            unsigned b = nhoods[i].nn_old[oldj];
            DataType dist = distance_->compare(
              features_.get_row(a), features_.get_row(b), dim);
            unsigned r = nhoods[a].insert(b,dist);
            if(r < params_.Check_K){uu += 2;}
            nhoods[b].insert(a,dist);
          }
        }
*/
	nhoods[i].join([&](unsigned i, unsigned j) {
		DataType dist = distance_->compare(
              		features_.get_row(i), features_.get_row(j), dim);
		++cc;
		unsigned r;
		r = nhoods[i].insert(j, dist);
		if (r < params_.Check_K) ++uu;
		nhoods[j].insert(i, dist);
		if (r < params_.Check_K) ++uu;
	});
        nhoods[i].found = uu > 0;
      }

    }

    void update (int paramL) {
      for (size_t i = 0; i < nhoods.size(); i++) {
          nhoods[i].nn_new.clear();
          nhoods[i].nn_old.clear();
          nhoods[i].rnn_new.clear();
          nhoods[i].rnn_old.clear();
          nhoods[i].radius = nhoods[i].pool.back().dist;
      }
      //find longest new
#pragma omp parallel for
      for(int i = 0; i < nhoods.size(); i++){
        if(nhoods[i].found){
          unsigned maxl = nhoods[i].Range + params_.S < nhoods[i].L ? nhoods[i].Range + params_.S : nhoods[i].L;
          unsigned c = 0;
          unsigned l = 0;
          while ((l < maxl) && (c < params_.S)) {
              if (nhoods[i].pool[l].flag) ++c;
              ++l;
          }
          nhoods[i].Range = l;
        }
        nhoods[i].radiusM = nhoods[i].pool[nhoods[i].Range-1].dist;
      }
#pragma omp parallel for
      for (int n = 0; n < nhoods.size(); ++n) {
          Neighbor &nhood = nhoods[n];
          std::vector<unsigned> &nn_new = nhood.nn_new;
          std::vector<unsigned> &nn_old = nhood.nn_old;
          for (unsigned l = 0; l < nhood.Range; ++l) {
              Point &nn = nhood.pool[l];
              Neighbor &nhood_o = nhoods[nn.id];  // nhood on the other side of the edge
              if (nn.flag) {
                  nn_new.push_back(nn.id);
                  if (nn.dist > nhood_o.radiusM) {
                      LockGuard guard(*nhood_o.lock);
                      nhood_o.rnn_new.push_back(n);
                  }
                  nn.flag = false;
              }
              else {
                  nn_old.push_back(nn.id);
                  if (nn.dist > nhood_o.radiusM) {
                      LockGuard guard(*nhood_o.lock);
                      nhood_o.rnn_old.push_back(n);
                  }
              }
          }
      }
      for (unsigned i = 0; i < nhoods.size(); ++i) {
          std::vector<unsigned> &nn_new = nhoods[i].nn_new;
          std::vector<unsigned> &nn_old = nhoods[i].nn_old;
          std::vector<unsigned> &rnn_new = nhoods[i].rnn_new;
          std::vector<unsigned> &rnn_old = nhoods[i].rnn_old;
          if (paramL && (rnn_new.size() > (unsigned int)paramL)) {
              random_shuffle(rnn_new.begin(), rnn_new.end());
              rnn_new.resize(paramL);
          }
          nn_new.insert(nn_new.end(), rnn_new.begin(), rnn_new.end());
          if (paramL && (rnn_old.size() > (unsigned int)paramL)) {
              random_shuffle(rnn_old.begin(), rnn_old.end());
              rnn_old.resize(paramL);
          }
          nn_old.insert(nn_old.end(), rnn_old.begin(), rnn_old.end());
      }
    }
    void refineGraph(){
      std::cout << " refineGraph" << std::endl;
      int iter = 0;
      clock_t s,f;
      s = clock();unsigned int l=100;
      while(iter++ < params_.build_epoches){
        join();//std::cout<<"after join"<<std::endl;
        update(l);
        f = clock();
        std::cout << "iteration "<< iter << " time: "<< (f-s)*1.0/CLOCKS_PER_SEC<<" seconds"<< std::endl;
      }
      //calculate_norm();
std::cout << "saving graph" << std::endl;
  /*    knn_graph.clear();

      for(size_t i = 0; i < nhoods.size(); i++){
        CandidateHeap can;
        for(size_t j = 0; j < params_.K; j++){
          Candidate<DataType> c(nhoods[i].pool[j].id,nhoods[i].pool[j].dist);
          can.insert(c);

        }
        while(can.size()<params_.K){
          unsigned id = rand() % nhoods.size();
          DataType dist = distance_->compare(features_.get_row(i), features_.get_row(id),features_.get_cols());
          Candidate<DataType> c(id, dist);
          can.insert(c);
        }
        knn_graph.push_back(can);
      }
*/
	g.resize(nhoods.size());
	M.resize(nhoods.size());
	gs.resize(nhoods.size());
	for(unsigned i = 0; i < nhoods.size();i++){
	    M[i] = nhoods[i].Range;
	    g[i].resize(nhoods[i].pool.size());
	    std::copy(nhoods[i].pool.begin(), nhoods[i].pool.end(), g[i].begin());
	    gs[i].resize(params_.K);
	    for(unsigned j = 0; j < params_.K;j++)
		gs[i][j] = g[i][j].id;
	}

    }

    void calculate_norm(){
	unsigned N = features_.get_rows();
	unsigned D = features_.get_cols();
	norms.resize(N);
#pragma omp parallel for
        for (int n = 0; n < N; ++n) {
	    norms[n] = distance_->norm(features_.get_row(n),D);
	}
    }
typedef std::set<Candidate<DataType>, std::greater<Candidate<DataType>> > CandidateHeap;
typedef std::vector<unsigned int> IndexVec;
	size_t getGraphSize(){return gs.size();}
	std::vector<unsigned> getGraphRow(unsigned row_id){
		std::vector<unsigned> row;
		if(gs.size() > row_id){
			for(unsigned i = 0; i < gs[row_id].size(); i++)row.push_back(gs[row_id][i]);
		}
		return row;
	}

protected:
    const Matrix<DataType> features_;
    const Distance<DataType>* distance_;
    const IndexParams params_;
    std::vector<std::vector<int> > knn_table_gt;
    std::vector<std::vector<Point> > g;
    std::vector<std::vector<unsigned> > gs;
    std::vector<unsigned> M;
    //std::vector<std::vector<int>> knn_graph;
    std::vector<CandidateHeap> knn_graph;
    std::vector<DataType> norms;
    std::vector<std::vector<int> > nn_results;
    DataType* Radius;
  };
#define USING_BASECLASS_SYMBOLS \
    using InitIndex<DataType>::distance_;\
    using InitIndex<DataType>::params_;\
    using InitIndex<DataType>::features_;\
    using InitIndex<DataType>::buildIndex;\
    using InitIndex<DataType>::knn_table_gt;\
    using InitIndex<DataType>::nn_results;\
    using InitIndex<DataType>::saveResults;\
    using InitIndex<DataType>::knn_graph;\
    using InitIndex<DataType>::refineGraph;\
    using InitIndex<DataType>::nhoods;\
    using InitIndex<DataType>::SP;\
    using InitIndex<DataType>::nnExpansion;\
    using InitIndex<DataType>::nnExpansion_kgraph;\
    using InitIndex<DataType>::g;\
    using InitIndex<DataType>::gs;\
    using InitIndex<DataType>::M;\
    using InitIndex<DataType>::norms;

}
#endif


// Copyright (c) 2016
// Author: Chrono Law
#include <std.hpp>
using namespace std;

#include <boost/version.hpp>
#if BOOST_VERSION <= 106400
#include <boost/uuid/sha1.hpp>
#else
#include <boost/uuid/detail/sha1.hpp>
#endif

#include <boost/iostreams/concepts.hpp>
#include <boost/iostreams/device/array.hpp>
#include <boost/iostreams/copy.hpp>
#include <boost/iostreams/stream.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/counter.hpp>
#include <boost/iostreams/device/back_inserter.hpp>

namespace io = boost::iostreams;
using namespace io;
using namespace boost;

///////////////////////////////////////

class sha1_filter final : public io::multichar_output_filter
{
public:
    typedef char char_type;
    //struct category:
    //    output,
    //    filter_tag,
    //    multichar_tag,
    //    closable_tag
    //    { };
private:
    boost::uuids::detail::sha1 sha;
public:
    sha1_filter() = default;
    ~sha1_filter() = default;
public:

    template<typename Sink>
    std::streamsize write(Sink&, const char_type* s, std::streamsize n)
    {
        sha.process_bytes(s, n);
        return n;
    }

    template<typename Sink>
    void close(Sink& snk)
    {
        unsigned int digest[5];
        sha.get_digest(digest);

        char_type* p = reinterpret_cast<char_type*>(digest);
        io::write(snk, p, sizeof(digest)/sizeof(char_type));
    }
};

BOOST_IOSTREAMS_PIPABLE(sha1_filter, 0);

///////////////////////////////////////

void case1()
{
    char str[] = "The Evil Within";
    string result;

    filtering_ostream out(
        sha1_filter() |
        io::back_inserter(result));

    io::write(out, str, 3);
    assert(result.empty());

    io::write(out, str + 3, 3);
    assert(result.empty());

    io::close(out);
    assert(result.size() == 20);
}

///////////////////////////////////////

template<typename Source, typename Sink>
std::streamsize
copy_no_close_impl(Source src, Sink snk,
              std::streamsize buffer_size )
{
    return io::detail::copy_operation<Source, Sink>(
        src , snk, buffer_size )
        ();
}

template<typename Source, typename Sink>
std::streamsize
copy_no_close( const Source& src, Sink& snk,
                  std::streamsize buffer_size =
                     io::default_device_buffer_size
                  BOOST_IOSTREAMS_ENABLE_IF_STREAM(Source)
                  BOOST_IOSTREAMS_ENABLE_IF_STREAM(Sink) )
{
    return copy_no_close_impl(
        io::detail::wrap(src),
        io::detail::wrap(snk),
        buffer_size );
}

void case2()
{
    char str[] = "The Evil Within";
    string result;

    stream<array_source> in(str);

    filtering_ostream out(
        sha1_filter() |
        io::back_inserter(result));

    copy_no_close(in, out);
    assert(in.is_open());

    io::close(in);
    io::close(out);

    assert(result.size() == 20);
}

///////////////////////////////////////

int main()
{
    std::cout << "hello iostreams sha1_filter" << std::endl;

    case1();
    case2();
}


//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  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)
//

#include <boost/cstdint.hpp>

namespace boost {
    namespace locale {
        namespace gnu_gettext {

            struct pj_winberger_hash {

                typedef uint32_t state_type;

                static const state_type initial_state = 0;

                static state_type update_state(state_type value,char c)
                {
                    value = (value << 4) + static_cast<unsigned char>(c);
                    uint32_t high = (value & 0xF0000000U);
                    if(high!=0)
                        value = (value ^ (high >> 24)) ^ high;
                    return value;
                }
                static state_type update_state(state_type value,char const *ptr)
                {
                    while(*ptr)
                        value = update_state(value,*ptr++);
                    return value;
                }
                static state_type update_state(state_type value,char const *begin,char const *end)
                {
                    while(begin!=end)
                        value = update_state(value,*begin++);
                    return value;
                }
            };

            inline pj_winberger_hash::state_type pj_winberger_hash_function(char const *ptr)
            {
                pj_winberger_hash::state_type state = pj_winberger_hash::initial_state;
                state = pj_winberger_hash::update_state(state,ptr);
                return state;
            }

            inline pj_winberger_hash::state_type pj_winberger_hash_function(char const *begin,char const *end)
            {
                pj_winberger_hash::state_type state = pj_winberger_hash::initial_state;
                state = pj_winberger_hash::update_state(state,begin,end);
                return state;
            }
        }
    }
}

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4


//  Copyright (c) 2009 Carl Barron
//
//  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)

#include <string>
#include <iostream>

#include <boost/detail/lightweight_test.hpp>
#include <boost/spirit/include/lex.hpp>
#include <boost/spirit/include/lex_lexertl.hpp>
#include <boost/spirit/include/phoenix_core.hpp>
#include <boost/spirit/include/phoenix_function.hpp>
#include <boost/phoenix/operator/self.hpp>

namespace lex = boost::spirit::lex;
namespace phoenix = boost::phoenix;

///////////////////////////////////////////////////////////////////////////////
struct square_impl
{
    template <class>
    struct result { typedef int type; };

    template <class A>
    int operator () (const A &x) const
    { return (x) * (x); }
};

phoenix::function<square_impl> const square = square_impl();

///////////////////////////////////////////////////////////////////////////////
template <class Lexer>
struct test_tokens : lex::lexer<Lexer>
{
    test_tokens()
    {
        a = "a";
        this->self = a [lex::_val = square(*lex::_start)];
    }

    lex::token_def<int> a;
};

struct catch_result
{
    template <class Token>
    bool operator() (Token const& x) const
    {
        BOOST_TEST(x.value().which() == 1);
        BOOST_TEST(boost::get<int>(x.value()) == 9409);  // 9409 == 'a' * 'a'
        return true;
    }
};

///////////////////////////////////////////////////////////////////////////////
int main()
{
    typedef lex::lexertl::token<std::string::iterator
      , boost::mpl::vector<int> > token_type;

    std::string in = "a";
    std::string::iterator first(in.begin());

    test_tokens<lex::lexertl::actor_lexer<token_type> > the_lexer;
    BOOST_TEST(lex::tokenize(first, in.end(), the_lexer, catch_result()));

    return boost::report_errors();
}


#include <v4r_config.h>
#include <v4r/common/miscellaneous.h>
#include <v4r/io/filesystem.h>
#include <v4r/io/eigen.h>
#include <v4r/registration/FeatureBasedRegistration.h>
#include <v4r/registration/fast_icp_with_gc.h>
#include <pcl/console/parse.h>
#include <pcl/filters/passthrough.h>
#include <pcl/recognition/cg/geometric_consistency.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <sstream>

#include <boost/graph/adjacency_list.hpp>
#include <boost/filesystem.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>

#ifdef HAVE_SIFTGPU
#include <v4r/features/sift_local_estimator.h>
#else
#include <v4r/features/opencv_sift_local_estimator.h>
#endif

struct CamConnect
{
    Eigen::Matrix4f transformation_;
    float edge_weight;
    size_t source_id_, target_id_;

    explicit CamConnect(float w) : edge_weight(w) { }

    CamConnect() : edge_weight(0.f)  {}

    bool operator<(const CamConnect& e) const { return edge_weight < e.edge_weight; }

    bool operator<=(const CamConnect& e) const { return edge_weight <= e.edge_weight; }

    bool operator>(const CamConnect& e) const { return edge_weight > e.edge_weight; }

    bool operator>=(const CamConnect& e) const { return edge_weight >= e.edge_weight ; }
};

class View
{
public:
    typedef pcl::PointXYZRGB PointT;

    std::vector<std::vector<float> >  sift_signatures_;
    std::vector<int> sift_keypoint_indices_;
    Eigen::Matrix4f camera_pose_;
    pcl::PointCloud<PointT>::Ptr  cloud_;

    View()
    {
        cloud_.reset(new pcl::PointCloud<PointT>());
    }
};

class scene_registration
{
private:
    typedef pcl::PointXYZRGB PointT;
    typedef flann::L1<float> DistT;

#ifdef HAVE_SIFTGPU
    cv::Ptr<SiftGPU> sift_;
#endif
    std::vector<View> grph_;

    typedef boost::property<boost::edge_weight_t, CamConnect> EdgeWeightProperty;
    typedef boost::adjacency_list < boost::vecS, boost::vecS, boost::undirectedS, size_t, EdgeWeightProperty> Graph;
    typedef boost::graph_traits < Graph >::vertex_descriptor Vertex;
    typedef boost::graph_traits < Graph >::edge_descriptor Edge;
    typedef boost::graph_traits<Graph>::vertex_iterator vertex_iter;
    typedef boost::property_map<Graph, boost::vertex_index_t>::type IndexMap;

    Graph gs_;

    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > absolute_poses_;

public:
    double chop_z_;

    scene_registration()
    {
        chop_z_ = std::numeric_limits<double>::max();
    }

    void
    calcEdgeWeightAndRefineTf (const typename pcl::PointCloud<PointT>::ConstPtr &cloud_src,
                                                            const typename pcl::PointCloud<PointT>::ConstPtr &cloud_dst,
                                                            const Eigen::Matrix4f &transform,
                                                            float &registration_quality,
                                                            Eigen::Matrix4f &refined_transform)
    {
        typename pcl::PointCloud<PointT>::Ptr cloud_src_wo_nan ( new pcl::PointCloud<PointT>());
        typename pcl::PointCloud<PointT>::Ptr cloud_dst_wo_nan ( new pcl::PointCloud<PointT>());

        pcl::PassThrough<PointT> pass;
        pass.setFilterLimits (0.f, 5.f);
        pass.setFilterFieldName ("z");
        pass.setInputCloud (cloud_src);
        pass.setKeepOrganized (true);
        pass.filter (*cloud_src_wo_nan);

        pcl::PassThrough<PointT> pass2;
        pass2.setFilterLimits (0.f, 5.f);
        pass2.setFilterFieldName ("z");
        pass2.setInputCloud (cloud_dst);
        pass2.setKeepOrganized (true);
        pass2.filter (*cloud_dst_wo_nan);

        float w_after_icp_ = std::numeric_limits<float>::max ();
        const float best_overlap_ = 0.75f;

        v4r::FastIterativeClosestPointWithGC<PointT> icp;
        icp.setMaxCorrespondenceDistance ( 0.02f );
        icp.setInputSource ( cloud_src_wo_nan );
        icp.setInputTarget ( cloud_dst_wo_nan );
        icp.setUseNormals (true);
        icp.useStandardCG (true);
        icp.setNoCG(true);
        icp.setOverlapPercentage (best_overlap_);
        icp.setKeepMaxHypotheses (5);
        icp.setMaximumIterations (10);
        icp.align (transform);
        w_after_icp_ = icp.getFinalTransformation ( refined_transform );

        if ( w_after_icp_ < 0 || !pcl_isfinite ( w_after_icp_ ) )
            w_after_icp_ = std::numeric_limits<float>::max ();
        else
            w_after_icp_ = best_overlap_ - w_after_icp_;

        //    transform = icp_trans; // refined transformation
        registration_quality = w_after_icp_;
    }

    void
    calcSiftFeatures (const pcl::PointCloud<PointT> &cloud_src,
                           pcl::PointCloud<PointT> &sift_keypoints,
                           std::vector<int> &sift_keypoint_indices,
                           std::vector<std::vector<float> > &sift_signatures,
                           std::vector<float> &sift_keypoint_scales)
    {
    #ifdef HAVE_SIFTGPU
        boost::shared_ptr < v4r::SIFTLocalEstimation<PointT> > estimator;
        estimator.reset (new v4r::SIFTLocalEstimation<PointT>());
        bool ret = estimator->estimate (cloud_src, sift_keypoints, sift_signatures, sift_keypoint_scales);
        (void)ret;
    #else
        (void)sift_keypoint_scales; //silences compiler warning of unused variable
        boost::shared_ptr < v4r::OpenCVSIFTLocalEstimation<PointT> > estimator;
        estimator.reset (new v4r::OpenCVSIFTLocalEstimation<PointT>);
        pcl::PointCloud<PointT> processed_foo;
        bool ret = estimator->estimate (cloud_src, processed_foo, sift_keypoints, sift_signatures);
    #endif
        estimator->getKeypointIndices( sift_keypoint_indices );
    }

    void
    computeAbsolutePosesRecursive (const Graph & grph,
                                  const Vertex start,
                                  const Eigen::Matrix4f &accum,
                                  std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & absolute_poses,
                                  std::vector<bool> &hop_list)
    {
        boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, gs_);
        boost::graph_traits<Graph>::out_edge_iterator ei, ei_end;
        for (boost::tie (ei, ei_end) = boost::out_edges (start, grph); ei != ei_end; ++ei)
        {
            Vertex targ = boost::target (*ei, grph);
            size_t target_id = boost::target (*ei, grph);

            if(hop_list[target_id])
               continue;

            hop_list[target_id] = true;
            CamConnect my_e = weightmap[*ei];
            Eigen::Matrix4f intern_accum;
            Eigen::Matrix4f trans = my_e.transformation_;
            if( my_e.target_id_ != target_id)
            {
                Eigen::Matrix4f trans_inv;
                trans_inv = trans.inverse();
                trans = trans_inv;
            }
            intern_accum = accum * trans;
            absolute_poses[target_id] = intern_accum;
            computeAbsolutePosesRecursive (grph, targ, intern_accum, absolute_poses, hop_list);
        }
    }

    void
    computeAbsolutePoses (const Graph & grph,
                         std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & absolute_poses,
                          const Eigen::Matrix4f &initial_transform = Eigen::Matrix4f::Identity())
    {
      size_t num_frames = boost::num_vertices(grph);
      absolute_poses.resize( num_frames );
      std::vector<bool> hop_list (num_frames, false);
      Vertex source_view = 0;
      hop_list[0] = true;
      Eigen::Matrix4f accum = initial_transform;
      absolute_poses[0] = accum;
      computeAbsolutePosesRecursive (grph, source_view, accum, absolute_poses, hop_list);
    }

    void addView(const pcl::PointCloud<PointT>::ConstPtr &cloud)
    {
        View view;

        pcl::PassThrough<PointT> pass;
        pass.setFilterLimits (0.f, chop_z_);
        pass.setFilterFieldName ("z");
        pass.setInputCloud (cloud);
        pass.setKeepOrganized (true);
        pass.filter (*view.cloud_);

        pcl::PointCloud<PointT>sift_keypoints;
        std::vector<float> sift_keypoint_scales;

        calcSiftFeatures( *view.cloud_, sift_keypoints, view.sift_keypoint_indices_, view.sift_signatures_, sift_keypoint_scales);
        grph_.push_back(view);
    }

    void buildGraph()
    {
        for (size_t view_a = 0; view_a < grph_.size(); view_a++)
        {
            for (size_t view_b = 0; view_b < view_a; view_b++)
            {
                std::vector<CamConnect> transforms;
                CamConnect edge;
                edge.source_id_ = view_b;
                edge.target_id_ = view_a;

                boost::shared_ptr<flann::Index<DistT> > flann_index;
                v4r::convertToFLANN<DistT>(grph_[view_b].sift_signatures_, flann_index );

                std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > sift_transforms =
                v4r::Registration::FeatureBasedRegistration<PointT>::estimateViewTransformationBySIFT(
                            *grph_[view_a].cloud_, *grph_[view_b].cloud_,
                            grph_[view_a].sift_keypoint_indices_, grph_[view_b].sift_keypoint_indices_,
                            grph_[view_a].sift_signatures_, grph_[view_b].sift_signatures_);

                for(size_t sift_tf_id = 0; sift_tf_id < sift_transforms.size(); sift_tf_id++)
                {
                    edge.transformation_ = sift_transforms[sift_tf_id];
                    transforms.push_back(edge);
                }

                size_t best_transform_id = 0;
                float lowest_edge_weight = std::numeric_limits<float>::max();
                for ( size_t trans_id = 0; trans_id < transforms.size(); trans_id++ )
                {
                    Eigen::Matrix4f icp_refined_trans;
                    calcEdgeWeightAndRefineTf(grph_[view_a].cloud_, grph_[view_b].cloud_, transforms[ trans_id ].transformation_, transforms[ trans_id ].edge_weight, icp_refined_trans);
                    transforms[ trans_id ].transformation_ = icp_refined_trans,
                    std::cout << "Edge weight is " << transforms[ trans_id ].edge_weight << " for edge connecting vertex " <<
                                 transforms[ trans_id ].source_id_ << " and " << transforms[ trans_id ].target_id_ << std::endl;

                    if(transforms[ trans_id ].edge_weight < lowest_edge_weight)
                    {
                        lowest_edge_weight = transforms[ trans_id ].edge_weight;
                        best_transform_id = trans_id;
                    }
                }
                boost::add_edge (transforms[best_transform_id].source_id_, transforms[best_transform_id].target_id_, transforms[best_transform_id], gs_);
            }
        }
    }

    void compute_mst()
    {
        boost::property_map<Graph, boost::edge_weight_t>::type weightmap = boost::get(boost::edge_weight, gs_);
        std::vector < Edge > spanning_tree;
        boost::kruskal_minimum_spanning_tree(gs_, std::back_inserter(spanning_tree));

        Graph grph_mst;
        std::cout << "Print the edges in the MST:" << std::endl;
        for (std::vector < Edge >::iterator ei = spanning_tree.begin(); ei != spanning_tree.end(); ++ei)
        {
            CamConnect my_e = weightmap[*ei];
            std::cout << "[" << source(*ei, gs_) << "->" << target(*ei, gs_) << "] with weight " << my_e.edge_weight << std::endl;
            boost::add_edge(source(*ei, gs_), target(*ei, gs_), weightmap[*ei], grph_mst);
        }

        computeAbsolutePoses(grph_mst, absolute_poses_);

        for(size_t view_id=0; view_id<absolute_poses_.size(); view_id++)
        {
            grph_[ view_id ].camera_pose_ = absolute_poses_ [ view_id ];
        }
    }

    std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> >
    getAbsolutePoses()
    {
        return absolute_poses_;
    }
};

int main (int argc, char ** argv)
{
    typedef pcl::PointXYZRGB PointT;
    std::string path;
    bool save_pose;
    double chop_z = std::numeric_limits<float>::max();
    pcl::console::parse_argument (argc, argv, "-path", path);
    pcl::console::parse_argument (argc, argv, "-chop_z", chop_z);
    pcl::console::parse_argument (argc, argv, "-save_pose", save_pose);

    std::cout << "Processing all point clouds in folder " << path;

    std::vector< std::string> sub_folder_names = v4r::io::getFoldersInDirectory(path);
    if(sub_folder_names.empty() )
        sub_folder_names.push_back("");

    for (size_t sub_folder_id=0; sub_folder_id < sub_folder_names.size(); sub_folder_id++)
    {
        const std::string path_sub = path + "/" + sub_folder_names[sub_folder_id];
        std::cout << "Processing all point clouds in folder " << path_sub;

        scene_registration sr;
        sr.chop_z_ = chop_z;

        std::vector < std::string > files_intern = v4r::io::getFilesInDirectory (path_sub, ".*.pcd", true);
        if (files_intern.empty())
        {
            std::cerr << "No files in directory: " << path_sub << ". Usage " << argv[0] << " -path <folder name> -rows <number of rows used for displaying files>" << std::endl;
            return -1;
        }

        for (size_t i=0; i < files_intern.size(); i++)
        {
            const std::string full_path = path_sub + "/"  + files_intern[i];
            pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
            pcl::io::loadPCDFile (full_path, *cloud);
            sr.addView(cloud);
        }

        sr.buildGraph();
        sr.compute_mst();
        std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > abs_poses = sr.getAbsolutePoses();

        if(save_pose)
        {
            for (size_t i=0; i < files_intern.size(); i++)
            {
                const std::string full_path = path_sub + "/"  + files_intern[i];
                pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
                pcl::io::loadPCDFile (full_path, *cloud);
                v4r::setCloudPose(abs_poses[i], *cloud);
                pcl::io::savePCDFileBinary(full_path, *cloud);
            }
        }
    }
}


#ifdef _DEBUG
#include <iostream>
#include <boost/format.hpp>
#endif

#include <vector>
#include <algorithm>
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include <data_type.hpp>
#include <image_algorithm/compare.hpp>
#include <image_algorithm/adjacent.hpp>
#include <image_algorithm/compare.hpp>
#include <image_algorithm/yrange5.hpp>
#include <gui.hpp>

// 2値座標系式から1値座標系式に変えながら和
// 指定した範囲の配列の和を返す
int yrange5::array_sum(return_type const& array_, int const x, int const y, int const height, int const width) const
{
	int sum = 0;
	for (int i = 0; i<height; ++i)
	{
		for (int j = 0; j < width; ++j)
		{
			auto const& target_point = array_[y + i][x + j];
			if (target_point.x == std::numeric_limits<int>::max() && target_point.y == std::numeric_limits<int>::max()) return -1;
			auto const point = target_point.y * width + target_point.x;
			sum += (point > 1000) ? 777 : point;
		}
	}
	return sum;
}

//cv::matの塊にする
cv::Mat yrange5::combine_image(answer_type_y const& answer)
{
	int const one_picx = data_.size.first / data_.split_num.first;
	int const one_picy = data_.size.second / data_.split_num.second;

	splitter sp;//どこからか持ってきてたsplitter
	split_image_type splitted = sp.split_image(data_);
	cv::Mat comb_pic(cv::Size(data_.size.first, data_.size.second), CV_8UC3);
	for (int i = 0; i < data_.split_num.second; ++i){
		for (int j = 0; j < data_.split_num.first; ++j){
			cv::Rect roi_rect(j*one_picx, i*one_picy, one_picx, one_picy);
			cv::Mat roi_mat(comb_pic, roi_rect);
			splitted[answer.points[i][j].y][answer.points[i][j].x].copyTo(roi_mat);
		}
	}
	return std::move(comb_pic.clone());
}

yrange5::yrange5(question_raw_data const& data, compared_type const& comp)
    : data_(data), comp_(comp), adjacent_data_(select_minimum(comp))
{
}

std::vector<answer_type_y> yrange5::operator() (std::vector<std::vector<std::vector<point_type>>> const& mother_matrix)
{

	auto const width = data_.split_num.first;
	auto const height = data_.split_num.second;
	constexpr int paramerter = 50;
	constexpr int yrange5_show_ans = 8;

	auto const exists = [height, width](point_type const& p)
	{
		return 0 <= p.x && p.x < width && 0 <= p.y && p.y < height;
	};

	std::vector<answer_type_y> to_y5;
	to_y5.reserve(width*height * 4);
	std::vector<answer_type_y> answer;
	answer.reserve(height*width * 2);
	splitter sp;

	for (int c = 0; c < mother_matrix.size(); ++c)
	{
		std::vector<kind_rgb>kind_rgb_vector;
		kind_rgb_vector.reserve(width*height);
		//kind_rgb選考
		for (int i = 0; i < height; i++) for (int j = 0; j < width; j++){
			int const & kind             = get_kind_num(data_, mother_matrix.at(c), j, i);
			uint_fast64_t const& score   = (width*height - kind + 1) * (range_evaluate(data_, comp_, mother_matrix.at(c), j, i));
			kind_rgb_vector.push_back(kind_rgb{ { j, i }, kind, score });
		}

		std::sort(kind_rgb_vector.begin(), kind_rgb_vector.end());
		kind_rgb_vector.resize(2);
		for (auto const& one_kind_rgb_vector:kind_rgb_vector)
		{
			std::vector<std::vector <point_type>> ans_temp(height, std::vector<point_type>(width));
			for (int i = 0; i < height; ++i) for (int j = 0; j < width; ++j)
			{
				ans_temp[i][j] = mother_matrix.at(c)[one_kind_rgb_vector.point.y + i][one_kind_rgb_vector.point.x + j];
			}
			to_y5.push_back(answer_type_y{ std::move(ans_temp), 0, cv::Mat() });
		}
	}
	std::sort(to_y5.begin(), to_y5.end(), [](answer_type_y a, answer_type_y b){return a.score < b.score; });
	to_y5.resize(yrange5_show_ans * 2);

	//yrange5メインループ
	for (int c = 0; c < to_y5.size(); ++c)
	{
		//重複して入っているもののうち，悪い方を{width,height}に置き換え,今使われていないものを返す
		std::vector<point_type> usable = std::move(duplicate_delete(comp_, to_y5.at(c).points));
		//It's show time!
		for (int i = 0; i < height; ++i)for (int j = 0; j < width; ++j)
		{
			std::vector<std::vector<point_type>> sorted_matrix(height, std::vector<point_type>(width, point_type{ width, height }));
			//全部並べ切れたら
			if (!try_matrix_copy(sorted_matrix, to_y5.at(c).points, point_type{ j, i }))
			{
				//抜けてるところを並べていく
				point_type const invalid_val = { width, height };
				int const u_height = height / 2 + 1;
				int const r_width = (width % 2 == 0) ? width / 2 : width / 2 + 1;
				int const d_height = (height % 2 == 0) ? height / 2 : height / 2 + 1;
				int const l_width = width / 2 + 1;

				//上に見ていく
				for (int i = 0; i < height - 1; ++i)
				{
					auto const& adjacent = sorted_matrix[height - 1 - i][width / 2];
					auto      & target = sorted_matrix[height - 2 - i][width / 2];
					if (target == invalid_val && adjacent != invalid_val) target = u_choose(comp_, adjacent, usable);
				}
				//下に見ていく
				for (int i = 0; i < height - 1; ++i)
				{
					auto const& adjacent = sorted_matrix[i][width / 2];
					auto      & target = sorted_matrix[i + 1][width / 2];
					if (target == invalid_val && adjacent != invalid_val) target = d_choose(comp_, adjacent, usable);
				}
				//右に見ていく
				for (int i = 0; i < width - 1; ++i)
				{
					auto const& adjacent = sorted_matrix[height / 2][i];
					auto      & target = sorted_matrix[height / 2][i + 1];
					if (target == invalid_val && adjacent != invalid_val) target = r_choose(comp_, adjacent, usable);
				}
				//左に見ていく
				for (int i = 0; i < width - 1; ++i)
				{
					auto const& adjacent = sorted_matrix[height / 2][width - 1 - i];
					auto      & target = sorted_matrix[height / 2][width - 2 - i];
					if (target == invalid_val && adjacent != invalid_val) target = l_choose(comp_, adjacent, usable);
				}

				//中心を除き上に向かってループ，右に見ていく
				for (int i = 0; i < u_height - 1; ++i) for (int j = 0; j < r_width - 1; ++j)
				{
					auto      & target = sorted_matrix[height / 2 - 1 - i][width / 2 + 1 + j];
					auto const& center = sorted_matrix[height / 2 - i][width / 2 + j];
					auto const& upper = sorted_matrix[height / 2 - i - 1][width / 2 + j];
					auto const& right = sorted_matrix[height / 2 - i][width / 2 + j + 1];

					if (target == invalid_val && (exists(center) && exists(upper) && exists(right)))
						target = ur_choose(comp_, upper, center, right, usable);
				}
				//中心を除き上に向かってループ，左に見ていく
				for (int i = 0; i < u_height - 1; ++i) for (int j = 0; j < l_width - 1; ++j)
				{
					auto      & target = sorted_matrix[height / 2 - i - 1][width / 2 - j - 1];
					auto const& center = sorted_matrix[height / 2 - i][width / 2 - j];
					auto const& upper = sorted_matrix[height / 2 - i - 1][width / 2 - j];
					auto const& left = sorted_matrix[height / 2 - i][width / 2 - j - 1];

					if (target == invalid_val && (exists(center) && exists(upper) && exists(left)))
						target = ul_choose(comp_, upper, left, center, usable);
				}
				//中心を除き下に向かってループ，右に見ていく
				for (int i = 0; i < d_height - 1; ++i) for (int j = 0; j < r_width - 1; ++j)
				{
					auto	  & target = sorted_matrix[height / 2 + i + 1][width / 2 + j + 1];
					auto const& center = sorted_matrix[height / 2 + i][width / 2 + j];
					auto const& lower = sorted_matrix[height / 2 + i + 1][width / 2 + j];
					auto const& right = sorted_matrix[height / 2 + i][width / 2 + j + 1];

					if (target == invalid_val && (exists(center) && exists(lower) && exists(right)))
						target = dr_choose(comp_, center, right, lower, usable);
				}
				//中心を除き下に向かってループ，左に見ていく
				for (int i = 0; i < d_height - 1; ++i) for (int j = 0; j < l_width - 1; ++j)
				{
					auto      & target = sorted_matrix[height / 2 + i + 1][width / 2 - j - 1];
					auto const& center = sorted_matrix[height / 2 + i][width / 2 - j];
					auto const& lower = sorted_matrix[height / 2 + i + 1][width / 2 - j];
					auto const& left = sorted_matrix[height / 2 + i][width / 2 - j - 1];

					if (target == invalid_val && (exists(center) && exists(lower) && exists(left)))
						target = dl_choose(comp_, left, center, lower, usable);
				}
				if (array_sum(sorted_matrix, 0, 0, height, width) == ((width*height - 1)*(width*height) / 2) && get_kind_num(data_, sorted_matrix, 0, 0) == width*height)
				{
					answer.push_back(answer_type_y{ sorted_matrix, form_evaluate(data_,comp_,sorted_matrix), cv::Mat() });
				}
			}
		}
	}

	//現段階で重複しているものは1つに絞る
	// unique()を使う準備としてソートが必要
	std::sort(answer.begin(), answer.end(), [](answer_type_y a, answer_type_y b){return a.points < b.points; });
	// unique()をしただけでは後ろにゴミが残るので、eraseで削除する
	answer.erase(std::unique(answer.begin(), answer.end()), answer.end());

	//無駄に多く返してもしょうがないので枝抜き
	size_t yrange5_ans = answer.size();
	std::sort(answer.begin(), answer.end(), [](answer_type_y a, answer_type_y b){return a.score < b.score; });
	if (answer.size() >= yrange5_show_ans) answer.resize(yrange5_show_ans);

#ifdef _DEBUG
//	//一枚のcv::Matにする
//#pragma omp parallel for
//	for (int c = 0; c < answer.size();++c)
//	{
//		answer.at(c).mat_image = std::move(combine_image(answer.at(c)));
//	}
	//for (auto const& one_answer : answer)
	//{
	//	for (int i = 0; i < one_answer.points.size(); ++i)
	//	{
	//		for (int j = 0; j < one_answer.points.at(0).size(); ++j)
	//		{
	//			auto const& data = one_answer.points[i][j];
	//			std::cout << boost::format("(%2d,%2d) ") % data.x % data.y;
	//		}
	//		std::cout << "\n";
	//	}
	//	std::cout << "score = " << one_answer.score << std::endl;
	//}
	//gui::show_image(data_, comp_, answer);
#endif
	std::cout << "There are " << yrange5_ans << " solutions by yrange5." << std::endl;
	return answer;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/detail/dispatch/adapted/common/pointer.hpp>
#include <boost/simd/detail/is_aligned.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = ::boost::dispatch;
  namespace bs = ::boost::simd;

  //------------------------------------------------------------------------------------------------
  // mask-load from an aligned pointer of int 32
  BOOST_DISPATCH_OVERLOAD ( aligned_load_
                          , (typename Target, typename Pointer)
                          , bs::avx2_
                          , bd::masked_pointer_<bd::scalar_<bd::ints32_<Pointer>>,std::true_type>
                          , bd::target_<bs::pack_<bd::ints32_<Target>,bs::avx_>>
                          )
  {
    using target  = typename Target::type;

    BOOST_FORCEINLINE target operator()(Pointer const& p, Target const& ) const
    {
      BOOST_ASSERT_MSG( boost::simd::detail::is_aligned(p, target::alignment)
                      , "boost::simd::aligned_load was performed on an "
                        "unaligned masked pointer of 32 bits integers"
                      );

      return _mm256_maskload_epi32( (std::int32_t const*)(p.get())
                                  , bs::as_logical_t<target>(p.mask())
                                  );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( aligned_load_
                          , (typename Target, typename Pointer)
                          , bs::avx2_
                          , bd::masked_pointer_<bd::scalar_<bd::ints32_<Pointer>>,std::false_type>
                          , bd::target_<bs::pack_<bd::ints32_<Target>,bs::avx_>>
                          )
  {
    using target  = typename Target::type;

    BOOST_FORCEINLINE target operator()(Pointer const& p, Target const& ) const
    {
      BOOST_ASSERT_MSG( boost::simd::detail::is_aligned(p, target::alignment)
                      , "boost::simd::aligned_load was performed on an "
                        "unaligned masked pointer of 32 bits integers"
                      );

      auto const& msk = p.mask();
      return  if_else ( bs::as_logical_t<target>(msk)
                      , aligned_load<target>( mask(p.get(),msk) )
                      , target(p.value())
                      );
    }
  };

  //------------------------------------------------------------------------------------------------
  // mask-load from an aligned pointer of int 64
  BOOST_DISPATCH_OVERLOAD ( aligned_load_
                          , (typename Target, typename Pointer)
                          , bs::avx2_
                          , bd::masked_pointer_<bd::scalar_<bd::ints64_<Pointer>>,std::true_type>
                          , bd::target_<bs::pack_<bd::ints64_<Target>,bs::avx_>>
                          )
  {
    using target  = typename Target::type;

    BOOST_FORCEINLINE target operator()(Pointer const& p, Target const& ) const
    {
      BOOST_ASSERT_MSG( boost::simd::detail::is_aligned(p, target::alignment)
                      , "boost::simd::aligned_load was performed on an "
                        "unaligned masked pointer of 64 bits integers"
                      );

      return _mm256_maskload_epi64( (long long int const*)(p.get())
                                  , bs::as_logical_t<target>(p.mask())
                                  );
    }
  };

  BOOST_DISPATCH_OVERLOAD ( aligned_load_
                          , (typename Target, typename Pointer)
                          , bs::avx2_
                          , bd::masked_pointer_<bd::scalar_<bd::ints64_<Pointer>>,std::false_type>
                          , bd::target_<bs::pack_<bd::ints64_<Target>,bs::avx_>>
                          )
  {
    using target  = typename Target::type;

    BOOST_FORCEINLINE target operator()(Pointer const& p, Target const& ) const
    {
      BOOST_ASSERT_MSG( boost::simd::detail::is_aligned(p, target::alignment)
                      , "boost::simd::aligned_load was performed on an "
                        "unaligned masked pointer of 64 bits integers"
                      );

      auto const& msk = p.mask();
      return  if_else ( bs::as_logical_t<target>(msk)
                      , aligned_load<target>( mask(p.get(),msk) )
                      , target(p.value())
                      );
    }
  };
} } }

#endif


#ifndef __RCS_FC_HH__
#define __RCS_FC_HH__
/********************************* TRICK HEADER *******************************
PURPOSE:
      (Describe the RCS Module On Board)
LIBRARY DEPENDENCY:
      ((../src/RcsFc.cpp))
*******************************************************************************/
#include <armadillo>
#include "aux.hh"

class RCS_FC {
  TRICK_INTERFACE(RCS_FC);

 public:
  RCS_FC();
  RCS_FC(const RCS_FC& other);

  RCS_FC& operator=(const RCS_FC& other);

  void initialize();

  void actuate();

  enum RCS_TYPE { NO_RCS = 0, ON_OFF_RCS = 2 };

  enum RCS_MODE {
    NO_CONTROL = 0,
    ALL_GEODETIC_EULUR_ANGLE_CONTROL = 1,
    THRUST_VECTOR_DIRECTION_AND_ROLL_ANGLE_CONTROL = 2,
    INCIDENCE_AND_ROLL_ANGLE_CONTROL = 3,
    GEODETIC_YAW_ANGLE_CONTROL = 4
  };

  void enable_rcs();
  void disable_rcs();

  bool isEnabled();

  void set_mode(int in);
  enum RCS_MODE get_rcs_mode();

  std::function<arma::vec3()> grab_UTBC;
  std::function<arma::vec3()> grab_WBICB;
  std::function<double()> grab_alphacomx;
  std::function<double()> grab_betacomx;

  std::function<double()> grab_alphacx;
  std::function<double()> grab_betacx;

  std::function<double()> grab_thtbdcx;
  std::function<double()> grab_psibdcx;
  std::function<double()> grab_phibdcx;

  double get_e_roll();
  double get_e_pitch();
  double get_e_yaw();

  /* Input File */
  void set_rcs_tau(double in);
  void set_thtbdcomx(double in);
  void set_psibdcomx(double in);
  void set_phibdcomx(double in);

 private:
  /* Internal Getter */

  /* Internal Initializers */
  void default_data();

  /* Internal Propagator / Calculators */

  /* Internal Calculators */

  /* Routing references */

  /* Input */
  double thtbdcomx; /* *o  (d)    Pitch angle command */
  double psibdcomx; /* *o  (d)    Yaw angle command */
  double phibdcomx; /* *o  (d)    Roll angle command */

  /* Constants */
  enum RCS_TYPE rcs_type; /* *o  (--)   Attitude control, see RCS_TYPE */
  enum RCS_MODE rcs_mode; /* *o  (--)   Attitude control, see RCS_MODE */

  double rcs_tau; /* *o  (s)    Slope of the switching function */

  // double  acc_gain;        [> *io  (N*s2/m) Acceleration gain of side
  // thrusters <] double  side_force_max;  [> *io  (N)    Maximum side force of
  // thruster <]

  /* Propagative Stats */

  /* Generating Outputs */
  double e_roll;  /* *o  (--)   Roll error signal */
  double e_pitch; /* *o  (--)   Pitch error signal */
  double e_yaw;   /* *o  (--)   Yaw error signal */

  /* Non-propagating Diagnostic Variables */
  /* These can be deleted, but keep to remain trackable in trick simulator */
};

#endif  // __RCS_FC_HH__


//
//  Copyright (c) 2009-2011 Artyom Beilis (Tonkikh)
//
//  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)
//
#define BOOST_LOCALE_SOURCE
#include <locale>
#include <string>
#include <ios>
#include <limits>
#include "formatter.hpp"
#include <boost/locale/formatting.hpp>
#include "all_generator.hpp"
#include "cdata.hpp"
#include <algorithm>
#include "predefined_formatters.hpp"

namespace boost {
namespace locale {
namespace impl_icu {

namespace details {
    template<typename V,int n=std::numeric_limits<V>::digits,bool integer=std::numeric_limits<V>::is_integer>
    struct cast_traits;

    template<typename v>
    struct cast_traits<v,7,true> {
        typedef int32_t cast_type;
    };
    template<typename v>
    struct cast_traits<v,8,true> {
        typedef int32_t cast_type;
    };

    template<typename v>
    struct cast_traits<v,15,true> {
        typedef int32_t cast_type;
    };
    template<typename v>
    struct cast_traits<v,16,true> {
        typedef int32_t cast_type;
    };
    template<typename v>
    struct cast_traits<v,31,true> {
        typedef int32_t cast_type;
    };
    template<typename v>
    struct cast_traits<v,32,true> {
        typedef int64_t cast_type;
    };
    template<typename v>
    struct cast_traits<v,63,true> {
        typedef int64_t cast_type;
    };
    template<typename v>
    struct cast_traits<v,64,true> {
        typedef int64_t cast_type;
    };
    template<typename V,int u>
    struct cast_traits<V,u,false> {
        typedef double cast_type;
    };

    // ICU does not support uint64_t values so fallback
    // to POSIX formatting
    template<   typename V,
                bool Sig=std::numeric_limits<V>::is_signed,
                bool Int=std::numeric_limits<V>::is_integer,
                bool Big=(sizeof(V) >= 8)
            >
    struct use_parent_traits
    {
        static bool use(V /*v*/) { return false; }
    };
    template<typename V>
    struct use_parent_traits<V,false,true,true>
    {
        static bool use(V v) { return static_cast<int64_t>(v) < 0; }
    };

}

class num_base {
protected:

    template<typename ValueType>
    static bool use_parent(std::ios_base &ios,ValueType v)
    {
        uint64_t flg = ios_info::get(ios).display_flags();
        if(flg == flags::posix)
            return true;
        if(details::use_parent_traits<ValueType>::use(v))
            return true;

        if(!std::numeric_limits<ValueType>::is_integer)
            return false;

        if(flg == flags::number && (ios.flags() & std::ios_base::basefield) != std::ios_base::dec) {
            return true;
        }
        return false;
    }
};

template<typename CharType>
class num_format : public std::num_put<CharType>, protected num_base
{
public:
    typedef typename std::num_put<CharType>::iter_type iter_type;
    typedef std::basic_string<CharType> string_type;
    typedef CharType char_type;
    typedef formatter<CharType> formatter_type;
    typedef std::auto_ptr<formatter_type> formatter_ptr;

    num_format(cdata const &d,size_t refs = 0) :
        std::num_put<CharType>(refs),
        loc_(d.locale),
        enc_(d.encoding)
    {
    }
protected:

    virtual iter_type do_put (iter_type out, std::ios_base &ios, char_type fill, long val) const
    {
        return do_real_put(out,ios,fill,val);
    }
    virtual iter_type do_put (iter_type out, std::ios_base &ios, char_type fill, unsigned long val) const
    {
        return do_real_put(out,ios,fill,val);
    }
    virtual iter_type do_put (iter_type out, std::ios_base &ios, char_type fill, double val) const
    {
        return do_real_put(out,ios,fill,val);
    }
    virtual iter_type do_put (iter_type out, std::ios_base &ios, char_type fill, long double val) const
    {
        return do_real_put(out,ios,fill,val);
    }

    #ifndef BOOST_NO_LONG_LONG
    virtual iter_type do_put (iter_type out, std::ios_base &ios, char_type fill, long long val) const
    {
        return do_real_put(out,ios,fill,val);
    }
    virtual iter_type do_put (iter_type out, std::ios_base &ios, char_type fill, unsigned long long val) const
    {
        return do_real_put(out,ios,fill,val);
    }
    #endif

private:

    template<typename ValueType>
    iter_type do_real_put (iter_type out, std::ios_base &ios, char_type fill, ValueType val) const
    {
        formatter_ptr formatter;

        if(use_parent<ValueType>(ios,val) || (formatter = formatter_type::create(ios,loc_,enc_)).get() == 0) {
            return std::num_put<char_type>::do_put(out,ios,fill,val);
        }

        size_t code_points;
        typedef typename details::cast_traits<ValueType>::cast_type cast_type;
        string_type const &str = formatter->format(static_cast<cast_type>(val),code_points);
        std::streamsize on_left=0,on_right = 0,points = code_points;
        if(points < ios.width()) {
            std::streamsize n = ios.width() - points;

            std::ios_base::fmtflags flags = ios.flags() & std::ios_base::adjustfield;

            //
            // We do not really know internal point, so we assume that it does not
            // exist. So according to the standard field should be right aligned
            //
            if(flags != std::ios_base::left)
                on_left = n;
            on_right = n - on_left;
        }
        while(on_left > 0) {
            *out++ = fill;
            on_left--;
        }
        std::copy(str.begin(),str.end(),out);
        while(on_right > 0) {
            *out++ = fill;
            on_right--;
        }
        ios.width(0);
        return out;

    }

    icu::Locale loc_;
    std::string enc_;

};  /// num_format

template<typename CharType>
class num_parse : public std::num_get<CharType>, protected num_base
{
public:
    num_parse(cdata const &d,size_t refs = 0) :
        std::num_get<CharType>(refs),
        loc_(d.locale),
        enc_(d.encoding)
    {
    }
protected:
    typedef typename std::num_get<CharType>::iter_type iter_type;
    typedef std::basic_string<CharType> string_type;
    typedef CharType char_type;
    typedef formatter<CharType> formatter_type;
    typedef std::auto_ptr<formatter_type> formatter_ptr;
    typedef std::basic_istream<CharType> stream_type;

    virtual iter_type do_get(iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,long &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get(iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,unsigned short &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get(iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,unsigned int &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get(iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,unsigned long &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get(iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,float &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get(iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,double &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get (iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,long double &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    #ifndef BOOST_NO_LONG_LONG
    virtual iter_type do_get (iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,long long &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    virtual iter_type do_get (iter_type in, iter_type end, std::ios_base &ios,std::ios_base::iostate &err,unsigned long long &val) const
    {
        return do_real_get(in,end,ios,err,val);
    }

    #endif

private:

    //
    // This is not really an efficient solution, but it works
    //
    template<typename ValueType>
    iter_type do_real_get(iter_type in,iter_type end,std::ios_base &ios,std::ios_base::iostate &err,ValueType &val) const
    {
        formatter_ptr formatter;
        stream_type *stream_ptr = dynamic_cast<stream_type *>(&ios);

        if(!stream_ptr || use_parent<ValueType>(ios,0) || (formatter = formatter_type::create(ios,loc_,enc_)).get()==0) {
            return std::num_get<CharType>::do_get(in,end,ios,err,val);
        }

        typedef typename details::cast_traits<ValueType>::cast_type cast_type;
        string_type tmp;
        tmp.reserve(64);

        CharType c;
        while(in!=end && (((c=*in)<=32 && (c>0)) || c==127)) // Assuming that ASCII is a subset
            ++in;

        while(tmp.size() < 4096 && in!=end && *in!='\n') {
            tmp += *in++;
        }

        cast_type value;
        size_t parsed_chars;

        if((parsed_chars = formatter->parse(tmp,value))==0 || !valid<ValueType>(value)) {
            err |= std::ios_base::failbit;
        }
        else {
            val=static_cast<ValueType>(value);
        }

        for(size_t n=tmp.size();n>parsed_chars;n--) {
            stream_ptr->putback(tmp[n-1]);
        }

        in = iter_type(*stream_ptr);

        if(in==end)
            err |=std::ios_base::eofbit;
        return in;
    }

    template<typename ValueType,typename CastedType>
    bool valid(CastedType v) const
    {
        typedef std::numeric_limits<ValueType> value_limits;
        typedef std::numeric_limits<CastedType> casted_limits;
        if(v < 0 && value_limits::is_signed == false)
            return false;

        static const CastedType max_val = value_limits::max();

        if(sizeof(CastedType) > sizeof(ValueType) && v > max_val)
            return false;

        if(value_limits::is_integer == casted_limits::is_integer) {
            return true;
        }
        if(value_limits::is_integer) { // and casted is not
            if(static_cast<CastedType>(static_cast<ValueType>(v))!=v)
                return false;
        }
        return true;
    }

    icu::Locale loc_;
    std::string enc_;

};

template<typename CharType>
std::locale install_formatting_facets(std::locale const &in,cdata const &cd)
{
    std::locale tmp=std::locale(in,new num_format<CharType>(cd));
    if(!std::has_facet<icu_formatters_cache>(in)) {
        tmp=std::locale(tmp,new icu_formatters_cache(cd.locale));
    }
    return tmp;
}

template<typename CharType>
std::locale install_parsing_facets(std::locale const &in,cdata const &cd)
{
    std::locale tmp=std::locale(in,new num_parse<CharType>(cd));
    if(!std::has_facet<icu_formatters_cache>(in)) {
        tmp=std::locale(tmp,new icu_formatters_cache(cd.locale));
    }
    return tmp;
}

std::locale create_formatting(std::locale const &in,cdata const &cd,character_facet_type type)
{
        switch(type) {
        case char_facet:
            return install_formatting_facets<char>(in,cd);
        case wchar_t_facet:
            return install_formatting_facets<wchar_t>(in,cd);
        #ifdef BOOST_HAS_CHAR16_T
        case char16_t_facet:
            return install_formatting_facets<char16_t>(in,cd);
        #endif
        #ifdef BOOST_HAS_CHAR32_T
        case char32_t_facet:
            return install_formatting_facets<char32_t>(in,cd);
        #endif
        default:
            return in;
        }
}

std::locale create_parsing(std::locale const &in,cdata const &cd,character_facet_type type)
{
        switch(type) {
        case char_facet:
            return install_parsing_facets<char>(in,cd);
        case wchar_t_facet:
            return install_parsing_facets<wchar_t>(in,cd);
        #ifdef BOOST_HAS_CHAR16_T
        case char16_t_facet:
            return install_parsing_facets<char16_t>(in,cd);
        #endif
        #ifdef BOOST_HAS_CHAR32_T
        case char32_t_facet:
            return install_parsing_facets<char32_t>(in,cd);
        #endif
        default:
            return in;
        }
}

} // impl_icu

} // locale
} //boost

// vim: tabstop=4 expandtab shiftwidth=4 softtabstop=4


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/function/bitwise_cast.hpp>
#include <boost/simd/detail/assert_utils.hpp>
#include <boost/simd/detail/overload.hpp>
#include <boost/simd/detail/brigand.hpp>
#include <boost/simd/detail/traits.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;

  BOOST_DISPATCH_OVERLOAD_IF( shift_left_
                            , (typename A0, typename A1, typename X)
                            , (detail::is_native<X>)
                            , bd::cpu_
                            , bs::pack_<bd::floating_<A0>,X>
                            , bd::scalar_<bd::fundamental_<A1>>
                            )
  {
    BOOST_FORCEINLINE A0 operator()(const A0& a0, const A1& a1) const BOOST_NOEXCEPT
    {
      BOOST_ASSERT_MSG(assert_good_shift<A0>(a1), "shift_left: a shift is out of range");
      return bitwise_cast<A0> ( shift_left( bitwise_cast<bd::as_integer_t<A0, signed>>(a0)
                                          , bd::as_integer_t<A0>(a1)
                                          )
                              );
    }
  };
} } }

#endif


#include "atomistic.hpp"
#include "formats.hpp"
#include "types.hpp"
#include "io.hpp"

#include <iostream>
#include <fstream>
#include <list>
#include <vector>
#include <string>

#include <boost/program_options.hpp>

#include <ctime>
time_t t = clock();

namespace po = boost::program_options;
namespace at = atomistic;
namespace units= at::units;
namespace stm = formats::stm;
using namespace types;

/***********************************************
  Declarations
***********************************************/

// Returns true, if command line arguments parse ok
bool parse(int ac, char* av[], po::variables_map& vm);

// Reads and parses the lists suppiled by command line
// and calls sum
bool readLists(types::String levelFileName,
             types::String cubeListFileName,
             types::String out,
             types::Real eMin,
             types::Real eMax,
             types::Real deltaE,
             types::Real FWHM,
             stm::StsCube::ModeFlag modeFlag,
             types::String modeParameter,
             types::Real cubeZ,
             bool psiSquared);

// Inserts info about energy levels in cube file descriptions
bool insertEnergyLevels(
        std::list<formats::WfnCube> &cubeList,
        formats::cp2k::Spectrum spectrum,
        types::Real eMin,
        types::Real eMax,
        types::Real sigma);

/***********************************************
  Implementations
***********************************************/

int main(int ac, char* av[]) {

    po::variables_map args;
    if (parse(ac, av, args)) {

        // Process mode-related input
        stm::StsCube::ModeFlag modeFlag;
        String modeParameter;

        if (args["mode"].as< String >() == "constant-z"){
            modeFlag = stm::StsCube::CONSTANT_Z;
            modeParameter = args["height"].as< String >() ;
        } else if (args["mode"].as< String >() == "z-profile"){
            modeFlag = stm::StsCube::PROFILE;
            modeParameter = args["z-profile"].as< String >() ;
        }

        readLists(args["levels"].as< types::String >(),
                args["cubelist"].as< types::String >(),
                args["out"].as< types::String >(),
                args["emin"].as< types::Real >(),
                args["emax"].as< types::Real >(),
                args["delta-e"].as< types::Real >(),
                args["FWHM"].as< types::Real >(),
                modeFlag,
                modeParameter,
                args["cubez"].as< types::Real >(),
                args["psisquared"].as< bool >()
                );

    }

    return 0;
}

bool readLists(types::String levelFileName,
             types::String cubeListFileName,
             types::String outFileName,
             types::Real eMin,
             types::Real eMax,
             types::Real deltaE,
             types::Real FWHM,
             stm::StsCube::ModeFlag modeFlag,
             types::String modeParameter,
             types::Real cubeZ,
             bool psiSquared){
    using namespace types;

    // Read energy levels
    std::cout << "Reading energy levels from " << levelFileName << "\n";
    formats::cp2k::Spectrum spectrum = formats::cp2k::Spectrum();
    spectrum.readFromCp2k(levelFileName.c_str());
    spectrum.setFermiZero();
    // Want spectrum in eV
    spectrum *= at::units::Ha / at::units::eV;

    // Read descriptions of cube files
    std::cout << "Reading list of cube files from " << cubeListFileName << "\n";
    std::ifstream cubeListFile;
    cubeListFile.open(cubeListFileName.c_str());
    if (!cubeListFile.is_open() )
        throw types::fileAccessError() << boost::errinfo_file_name(cubeListFileName);

    std::list<formats::WfnCube> cubeList;
    types::String fileName;
    while (getline(cubeListFile, fileName)) {
        formats::WfnCube t = formats::WfnCube();
        t.readDescription(fileName);
        cubeList.push_back(t);

    }
    cubeListFile.close();
    insertEnergyLevels(cubeList, spectrum, eMin, eMax, FWHM);

    std::cout << "Done with preparation\n------------\n";
    stm::StsCube mySts = stm::StsCube(
        cubeList,
        eMin,
        eMax,
        deltaE,
        FWHM,
        modeFlag,
        modeParameter,
        cubeZ,
        psiSquared);
    std::cout << "Writing STS cube file " << outFileName << "\n";
    mySts.writeCubeFile(outFileName);

    return true;
}

bool insertEnergyLevels(
        std::list<formats::WfnCube> &cubeList,
        formats::cp2k::Spectrum spectrum,
        types::Real eMin,
        types::Real eMax,
        types::Real FWHM){

    std::list<formats::WfnCube> newCubeList;
    // We also take levels that are sigma*3 away from the border
    types::Real delta = 3 * FWHM;

    for(Uint spin = 0; spin < spectrum.spins.size(); ++spin){
        at::EnergyLevels levels = spectrum.spins[spin];

        for(Uint level = 1; level <= levels.count(); ++level){
            Real energy = levels.getLevel(level);

            // If we need this level ...
            if( energy >= eMin - delta && energy <= eMax + delta){

                // Find cube file
                bool found = false;
                std::list<formats::WfnCube>::iterator
                    cubeIt = cubeList.begin(),
                           cubeEnd = cubeList.end();
                while(cubeIt != cubeEnd){
                    if(cubeIt->wfn == level && cubeIt->spin == spin +1){
                        cubeIt->setEnergy(energy);
                        newCubeList.push_back(*cubeIt);

                        // exit cube search
                        found = true;
                        std::cout << "Found cube file for energy level "
                            << level << " at "
                            << energy << " eV\n";
                        break;
                    }
                    else ++cubeIt;

                }
                if(!found) std::cout << "Missing cube file for energy level "
                    << level << " at "
                        << energy << " eV\n";
            }
        }
    }

    cubeList = newCubeList;

    return true;
}

bool parse(int ac, char* av[], po::variables_map& vm) {

    std::string input_file;
    // Declare regular options
    po::options_description desc("Allowed options");
    desc.add_options()
    ("help,h", "produce help message")
    ("version,v", "print version information")
    ("input-file,i", po::value<types::String>(&input_file), "Input file specifying all or several of the following options")
    ("levels", po::value<types::String>(), "CP2K output file containing energy levels")
    ("cubelist", po::value<types::String>(), "file with list of extrapolated wave function cubes from CP2K")
    ("psisquared", po::value<bool>()->default_value(false), "Whether the cube files contain the square of the wave function (and not the wave function itself)")
    ("out", po::value<types::String>()->default_value("sts.cube"), "filename of sts cube file")
    ("emin", po::value<types::Real>(), "Minimum bias for STS [V]")
    ("emax", po::value<types::Real>(), "Maximum bias for STS [V]")
    ("cubez", po::value<types::Real>()->default_value(0.0), "z-dimension [a0] of cube file to create")
    ("delta-e", po::value<types::Real>()->default_value(0.01), "Bias step for STS [V]")
    ("FWHM", po::value<types::Real>()->default_value(0.2), "FWHM of Gaussian broadening [V]. FWHM = 2.355 sigma.")
    ("mode", po::value<types::String>()->default_value("constant-z"), "Scanning mode, may be 'constant-z' or 'z-profile'")
    ("height", po::value<types::String>(), "Height [a0] above top surface, where STS shall be performed (mode = 'constant-z')")
    ("z-profile", po::value<types::String>(), ".igor file containing the z-profile [a.u.] on which to perform the STS, e.g. STM profile (mode = 'z-profile')")
    ;

    // Register positional options
    po::positional_options_description p;
    p.add("input-file",-1);

    // Parse
    po::store(po::command_line_parser(ac,av).
              options(desc).positional(p).run(), vm);
    po::notify(vm);

    // If specified, try to parse conig file
    if (vm.count("input-file")){
        std::ifstream ifs(input_file.c_str());
       if(!ifs) throw types::fileAccessError() << boost::errinfo_file_name(input_file);
       store(parse_config_file(ifs, desc), vm);
       notify(vm);
    }

    // Check if all necessary things are specified
    if (	vm.count("help") ||
            !vm.count("levels") ||
            !vm.count("cubelist") ||
            !vm.count("emax") ||
            !vm.count("emin") ||
            !vm.count("delta-e") ||
            !vm.count("FWHM") ||
            !vm.count("mode")
       ){
        std::cout << "Usage: sts [options]\n";
        std::cout << desc << "\n";
    } else if (vm.count("version")) {
        std::cout << "Mar 1st 2012\n";
    } else if (vm["mode"].as< String >() == "constant-z" && !vm.count("height")) {
                std::cout << "Error: Need to specify 'height' for mode='constant-z'.\n";
    } else if (vm["mode"].as< String >() == "z-profile" && !vm.count("z-profile")) {
                std::cout << "Error: Need to specify 'zprofile' for mode='profile'.\n";
    } else if (vm["emin"].as< Real >() > vm["emax"].as< Real >()){
        std::cout << "Error: emin > emax\n";
    } else {
        return true;
    }

    return false;
}


#define NPY_NO_DEPRECATED_API NPY_1_11_API_VERSION

#include <iostream>
#include <stdint.h>
#include <alloca.h>
#include <Eigen/Core>
#include <boost/python.hpp>
#include <boost/python/numeric.hpp>
#include <boost/python/ptr.hpp>
#include <Python.h>
#include <numpy/ndarrayobject.h>
//#include <numpy/arrayobject.h>

using namespace Eigen;
using namespace std;
using namespace boost::python;

#if PY_VERSION_HEX >= 0x03000000
void *
#else
void
#endif
init_numpy(){
    //Py_Initialize;
    import_array();
}

//original comment:
//"TODO if we aren't outputting gamma, don't need to write it to memory (just
//need t and t+1), so we can save the stack array for each HMM at the cost of
//a branch"

/*struct double_to_python_float
{
    static PyObject* convert(double const& d)
      {
        return boost::python::incref(
          boost::python::object(d).ptr());
      }
};*/

//numpy scalar converters.
template <typename T, NPY_TYPES NumPyScalarType>
struct enable_numpy_scalar_converter
{
  enable_numpy_scalar_converter()
  {
    // Required NumPy call in order to use the NumPy C API within another
    // extension module.
    // import_array();
    init_numpy();

    boost::python::converter::registry::push_back(
      &convertible,
      &construct,
      boost::python::type_id<T>());
  }

  static void* convertible(PyObject* object)
  {
    // The object is convertible if all of the following are true:
    // - is a valid object.
    // - is a numpy array scalar.
    // - its descriptor type matches the type for this converter.
    return (
      object &&                                                    // Valid
      PyArray_CheckScalar(object) &&                               // Scalar
      PyArray_DescrFromScalar(object)->type_num == NumPyScalarType // Match
    )
      ? object // The Python object can be converted.
      : NULL;
  }

  static void construct(
    PyObject* object,
    boost::python::converter::rvalue_from_python_stage1_data* data)
  {
    // Obtain a handle to the memory block that the converter has allocated
    // for the C++ type.
    namespace python = boost::python;
    typedef python::converter::rvalue_from_python_storage<T> storage_type;
    void* storage = reinterpret_cast<storage_type*>(data)->storage.bytes;

    // Extract the array scalar type directly into the storage.
    PyArray_ScalarAsCtype(object, storage);

    // Set convertible to indicate success.
    data->convertible = storage;
  }
};

// TODO openmp version

numeric::array run(dict& data, dict& model, numeric::array& forward_messages){
    //TODO: check if parameters are null.
    //TODO: check that dicts have the required members.
    //TODO: check that all parameters have the right sizes.
    //TODO: i'm not sending any error messages.

    numeric::array alldata = extract<numeric::array>(data["data"]); //multidimensional array, so i need to keep extracting arrays.
    int bigT = len(alldata[0]); //this should be the number of columns in the alldata object. i'm assuming is 2d array.
    int num_subparts = len(alldata);

    numeric::array allresources = extract<numeric::array>(data["resources"]);

    numeric::array starts = extract<numeric::array>(data["starts"]);
    int num_sequences = len(starts);

    numeric::array lengths = extract<numeric::array>(data["lengths"]);

    numeric::array learns = extract<numeric::array>(model["learns"]);
    int num_resources = len(learns);

    numeric::array forgets = extract<numeric::array>(model["forgets"]);

    numeric::array guesses = extract<numeric::array>(model["guesses"]);

    numeric::array slips = extract<numeric::array>(model["slips"]);

    double prior = extract<double>(model["prior"]);

    Array2d initial_distn;
    initial_distn << 1-prior, prior;

    MatrixXd As(2,2*num_resources);
    for (int n=0; n<num_resources; n++) {
        double learn = extract<double>(learns[n]);
        double forget = extract<double>(forgets[n]);
        As.col(2*n) << 1-learn, learn;
        As.col(2*n+1) << forget, 1-forget;
    }

    // forward messages
    //numeric::array all_forward_messages = extract<numeric::array>(forward_messages);
    double * forward_messages_temp = new double[2*bigT];
    for (int i=0; i<2; i++) {
        for (int j=0; j<bigT; j++){
            forward_messages_temp[i* bigT +j] = extract<double>(forward_messages[i][j]);
        }
    }

    //// outputs

    double * all_predictions = new double[2*bigT];
    Map<Array2Xd,Aligned> predictions(all_predictions,2,bigT);

    /* COMPUTATION */

    for (int sequence_index=0; sequence_index < num_sequences; sequence_index++) {
        // NOTE: -1 because Matlab indexing starts at 1
        int64_t sequence_start = extract<int64_t>(starts[sequence_index]) - 1;
        int64_t T = extract<int64_t>(lengths[sequence_index]);

        //int16_t *resources = allresources + sequence_start;
        Map<MatrixXd> forward_messages(forward_messages_temp + 2*sequence_start,2,T);
        Map<MatrixXd> predictions(all_predictions + 2*sequence_start,2,T);

        predictions.col(0) = initial_distn;
        for (int t=0; t<T-1; t++) {
            int64_t resources_temp = extract<int64_t>(allresources[sequence_start+t]);
            predictions.col(t+1) = As.block(0,2*(resources_temp-1),2,2) * forward_messages.col(t);
        }
    }

    npy_intp all_predictions_dims[2] = {2,bigT}; //TODO: just put directly this array into the PyArray_SimpleNewFromData function?
    PyObject * all_predictions_pyObj = PyArray_New(&PyArray_Type, 2, all_predictions_dims, NPY_DOUBLE, NULL, all_predictions, 0, NPY_ARRAY_CARRAY, NULL);
    boost::python::handle<> all_predictions_handle( all_predictions_pyObj );
    boost::python::numeric::array all_predictions_arr( all_predictions_handle );

    delete all_predictions;
    delete forward_messages_temp;

    return(all_predictions_arr);
}

BOOST_PYTHON_MODULE(predict_onestep_states){
    //import_array();
    init_numpy();
    numeric::array::set_module_and_type("numpy", "ndarray");
    //to_python_converter<double, double_to_python_float>();
    enable_numpy_scalar_converter<boost::int8_t, NPY_INT8>();
    enable_numpy_scalar_converter<boost::int16_t, NPY_INT16>();
    enable_numpy_scalar_converter<boost::int32_t, NPY_INT32>();
    enable_numpy_scalar_converter<boost::int64_t, NPY_INT64>();

    def("run", run);

}


#include <iostream>
#include <fstream>
#include <boost/program_options.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/core/eigen.hpp>
#include <boost/format.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>

#include <vector>
#include <string>

#include <sstream>
#include <boost/filesystem.hpp>
#include <algorithm>
#include <iterator>
#include <math.h>

using namespace std;
using namespace cv;

int loadPoses(string file_name, vector<Eigen::Matrix4d, Eigen::aligned_allocator<Eigen::Matrix4d> > &poses) {
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[3] [4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
                   &P[0][0], &P[0][1], &P[0][2], &P[0][3],
                   &P[1][0], &P[1][1], &P[1][2], &P[1][3],
                   &P[2][0], &P[2][1], &P[2][2], &P[2][3] ) == 12) {
            Eigen::Matrix4d T;
            T << P[0][0], P[0][1], P[0][2], P[0][3],
            P[1][0], P[1][1], P[1][2], P[1][3],
            P[2][0], P[2][1], P[2][2], P[2][3],
            0, 0, 0, 1;
            poses.push_back(T);
        }
    }
    fclose(fp);
    return 1;

}

int loadNdtPoses(string file_name, vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &pose_xyz,vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &pose_rpy) {
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double P[6];
        if (fscanf(fp, "%lf %lf %lf %lf %lf %lf",
                   &P[0], &P[1], &P[2], &P[3], &P[4], &P[5] ) == 6) {
            Eigen::Vector3d xyz,rpy;
            xyz<<P[0],P[1],P[2];
            rpy<<P[3],P[4],P[5];
            pose_rpy.push_back(rpy);
            pose_xyz.push_back(xyz);
        }
    }
    fclose(fp);
    return 1;

}

int loadGpsPoses(string file_name, vector<double> &stamp,vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > &pose,vector<double> &yaws) {
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp)
        return  0;
    while (!feof(fp)) {
        double time;
        double P[4];
        if (fscanf(fp, "%lf %lf %lf %lf %lf",
                   &time, &P[0], &P[1], &P[2], &P[3]) == 5) {
            Eigen::Vector3d p;
            p<<P[0],P[1],P[2];
            stamp.push_back(time);
            pose.push_back(p);
            yaws.push_back(P[3]);
        }
    }

    fclose(fp);
    return 1;

}

void loadStamp(string file_name, vector<double>& stamp){
    FILE *fp = fopen(file_name.c_str(), "r");
    if (!fp){
        cout<<"open file fail: "<<file_name<<endl;
        return  ;
    }
    while (!feof(fp)) {
        double timestamp;
        if (fscanf(fp, "%lf", &timestamp ) == 1) {
            stamp.push_back(timestamp);
        }
    }
    fclose(fp);
    return ;
}

bool CheckGPS(Eigen::Vector3d pose){//python在处理时，如果某一帧gps是失锁的，就会赋值-1.
    if((pose(0)==-1.0)&&(pose(1)==-1.0)&&(pose(2)==-1.0)){
        return false;
    }else{
        return true;
    }
}

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

    ofstream ofs;
    cv::FileStorage fsSettings("../config/config.yaml", cv::FileStorage::READ);
    const string gpsfile=fsSettings["gpsfile"];
    const string velodyneStampFile=fsSettings["velodyneStampFile"];
    const string velodynePoseFile=fsSettings["velodynePoseFile"];
    const string outGTFile=fsSettings["outGTFile"];
    const string outPoseFile=fsSettings["outPoseFile"];

#if 1   //transform gps
    vector<double> gpsstamp;
    vector<double> lvinstamp;
    vector<Eigen::Vector3d,Eigen::aligned_allocator<Eigen::Vector3d> > gpspose;
    vector<Eigen::Matrix4d,Eigen::aligned_allocator<Eigen::Matrix4d> > lvinspose;
    vector<double> yaws;
    loadGpsPoses(gpsfile,gpsstamp,gpspose,yaws);
    loadStamp(velodyneStampFile,lvinstamp);
    loadPoses(velodynePoseFile,lvinspose);
    cout<<"load done"<<endl;
    cout<<"gpsstamp.size() " << gpsstamp.size()<<" gpspose.size() "<<gpspose.size()<<" lvinstamp.size() "<<lvinstamp.size()<<endl;
    ofs.open(outGTFile,std::ios::out);
    ofstream lvinsofs;
    lvinsofs.open(outPoseFile,std::ios::out);

    Eigen::Quaterniond q;

    for(int i=0;i<lvinstamp.size();i++){//给每个激光帧打上gps的pose
        bool gpsvalid=true;
        int index=0;
        int j=0;
        while(gpsstamp[j]<=lvinstamp[i]){//找左边的帧
            j++;
            if(j==gpsstamp.size())
                break;
        }
        Eigen::Vector3d ap;//每一帧激光对应的真值
        if(j==0){//当激光数据比gps先到
            ap = gpspose[0];
        }else if(j==gpsstamp.size()){//当GPS结束，激光没有结束
            ap = gpspose[gpspose.size()-1];
        }else{//正常帧
            double a = (lvinstamp[i]-gpsstamp[j-1])/(gpsstamp[j]-gpsstamp[j-1]);
            ap << gpspose[j-1](0)+a*(gpspose[j](0)-gpspose[j-1](0)),
                  gpspose[j-1](1)+a*(gpspose[j](1)-gpspose[j-1](1)),
                  gpspose[j-1](2)+a*(gpspose[j](2)-gpspose[j-1](2));

            if(CheckGPS(gpspose[j-1])&&(!CheckGPS(gpspose[j]))){
                ap=gpspose[j-1];
            }else if(CheckGPS(gpspose[j])&&(!CheckGPS(gpspose[j-1]))){
                ap=gpspose[j];
            }
        }
        if((ap(0)==-1)&&(ap(1)==-1)&&(ap(2)==-1))
            gpsvalid=false;

        Eigen::Matrix3d rotation_matrix=lvinspose[i].block(0,0,3,3);
        q=Eigen::Quaterniond(rotation_matrix);
        //if(gpsvalid)
        lvinsofs<<setprecision(16)<<lvinstamp[i]<<" "<<setprecision(8)<<lvinspose[i](0,3)<<" "<<lvinspose[i](1,3)<<" "<<lvinspose[i](2,3)<<" "
          <<q.x()<<" "<<q.y()<<" "<<q.z()<<" "<<q.w()<<endl;
        if(gpsvalid)
            ofs<<setprecision(16)<<lvinstamp[i]<<" "<<setprecision(8)<<ap(0)<<" "<<ap(1)<<" "<<ap(2)<<" 0 0 0 1"<<endl;
    }
    ofs.close();

#endif

}


// Copyright Louis Dionne 2013-2016
// 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/mpl11/list.hpp>

struct is_even {
    using type = is_even;
    template <typename N>
    using apply = boost::mpl11::bool_<N::value % 2 == 0>;
};

using list = boost::mpl11::list<
    <%= (1..input_size).map { |i| "boost::mpl11::int_<#{i}>" }.join(', ') %>
>;

using result = boost::mpl11::filter<is_even, list>::type;

int main() { }