/////////////////////////////////////////////////////////////////////////////////////////////
// Copyright (c) 2021 Andreas Milton Maniotis.
//
// Email: andreas.maniotis@gmail.com
//
// 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)
/////////////////////////////////////////////////////////////////////////////////////////////

#pragma once

#include "../dictionary.hpp"
#include "../term_algebra.hpp"
#include "./record.hpp"

#include <type_traits>
#include <boost/core/demangle.hpp>
#include <iostream>

namespace aml::adt
{
    template<  typename Symbol
            ,  template<typename... > class SubType
            >
    using subtype  =  aml::entry<Symbol, aml::function<SubType> >;

    template<typename... SubType>
    struct signature
    {
    private:

        template<typename... X>
        struct linker
        {
            template<typename Symbol>
            using lookup  =  typename dictionary<SubType...>::
                             template lookup<Symbol>::
                             template apply_to<  linker<X...>,  X... >;

            template<typename Symbol>
            using entry = aml::entry< Symbol, lookup<Symbol> >;
        };

    public:

        template<typename... X>
        struct adt
        :    public record< typename linker<X...>::template entry< typename SubType::key >... >
        {
        private:
            using record_t  =  record<  typename linker<X...>::
                                        template entry< typename SubType::key >...  >;

            struct construct_from_permuted_record {};

            template<typename Record>
            adt(construct_from_permuted_record,  Record&& r)
                : record_t( static_cast<Record&&>(r).template extract< record_t >() )
            { }

        public:

            using signature  =  signature<SubType...>;

            template<typename Symbol>
            using sub  =  typename linker<X...>::template entry<Symbol>;

            adt()            =  default;
            adt(adt const&)  =  default;
            adt(adt&& )      =  default;

            template<  typename... SubAdt,
                       // sfinae away the case when the default copy- or move-constructor shall be called
                       typename  =  typename bool_<    ! std::is_same< list<SubAdt&&... >, list< adt const&   > >::value &&
                                                       ! std::is_same< list<SubAdt&&... >, list< adt &        > >::value &&
                                                       ! std::is_same< list<SubAdt&&... >, list< adt const && > >::value &&
                                                       ! std::is_same< list<SubAdt&&... >, list< adt &&       > >::value
                                                  >::sfinae
                    >
            adt(SubAdt&&... sub_adt)
                : adt( construct_from_permuted_record{}, make_record( static_cast<SubAdt&&>(sub_adt)... ) )
            { }

            record_t const& record() const &   { return static_cast<record_t const&>(*this); }
            record_t&&      record()       &&  { return static_cast<record_t&&>(*this); }
            record_t&       record()       &   { return static_cast<record_t&>(*this); }
            record_t const& crecord() const    { return static_cast<record_t const&>(*this); }

            template< typename T >
            auto const& cref()  const &
            {
                return record().template cref< std::decay_t<T> >();
            }

            template< typename T >
            auto const& ref()  const &
            {
                return cref<T>();
            }

            template< typename T >
            auto& ref()  &
            {
                return record().template ref<T>();
            }

            template< typename T >
            auto&& rref()
            {
                return record().template rref< std::decay_t<T> >();
            }

            template< typename T >
            auto&& ref() &&
            {
                return record().template rref< std::decay_t<T> >();
            }

            friend bool operator==(adt const& lhs, adt const& rhs)
            {
                return lhs.crecord() == rhs.crecord();
            }

            friend bool operator!=(adt const& lhs, adt const& rhs)
            {
                return ! ( lhs == rhs );
            }
        };

    };

}

/*

    template<typename... SubType>
    struct link
    {
    private:

        using lookup = dictionary<SubType...>;

        static_assert(lookup::size() == sizeof...(SubType),
                      "Symbols for sub types must be unique!");

        using this_linker     =  link<SubType...>;

    public:

        template< typename... X >
        class adt
        :
            public SubType::value::template apply_to< this_linker, X... >...
        {
        private:
            using this_type  =  adt< X... >;

            template< typename SubType_ >
            using symbol  =  typename SubType_::key;

            template< typename SubType_ >
            using instance  =  typename SubType_::value::template apply_to< this_linker, X...>;

            template< typename SubType_ >
            using entry  =  aml::entry<  symbol< SubType_>,  instance< SubType_ > >;

            using data_t  =  record< entry<SubType>... >;

            data_t data_;

        public:
            using signature  =  lookup;

            template<typename Symbol>
            using factor  =  typename signature::template lookup<Symbol>::template apply_to<this_linker, X...>;

            template<  typename... Args
                    ,  typename  =  typename  // exclude copy and move construction
                                    none<  is_same<  list< Args... >, list< this_type const & >  >
                                        ,  is_same<  list< Args... >, list< this_type & >  >
                                        ,  is_same<  list< Args... >, list< this_type && > >
                                        >::sfinae
                >
            explicit adt( Args&&... args )
            :   instance<SubType>(static_cast<Args&&>(args))...
            {}

            adt()
            :   instance<SubType>()...
            { }

            adt( adt const &  other)
            :   instance<SubType>( static_cast< instance<SubType> const & >(other))...
            { }

            adt( adt &&  other)
            :   instance<SubType>(static_cast< instance<SubType>&& >(other))...
            { }

            template< typename S >
            auto const&  operator[](const S&)  const &
            {
                return data_.template cref< std::decay_t<S> >();
            }

            template< typename S >
            auto& operator[](const S&)  &
            {
                return data_.template ref< std::decay_t<S> >();
            }

            template< typename S >
            auto&& operator[](S const&)  &&
            {
                return data_.template rref< std::decay_t<S> >();
            }

            template< typename T >
            auto const& cref()  const &
            {
                return data_.template cref< std::decay_t<T> >();
            }

            template< typename T >
            auto const& ref()  const &
            {
                return cref<T>();
            }

            template< typename T >
            auto& ref()  &
            {
                return ref<T>();
            }

            template< typename T >
            auto&& rref()
            {
                return data_.template rref< std::decay_t<T> >();
            }

            template< typename T >
            auto&& ref() &&
            {
                return data_.template rref< std::decay_t<T> >();
            }

        };

    };

}
*/


#include "dwag.h"

#include <iostream> // for cout
#include <fstream>  // for ifstream
#include "zlib.h"  // for gzFile; presumes zstd library is installed
#include "gmm.h"

#define MIS_MATCH_CUT   5
#define MIN_LEVEL_CUT   30
#define MATCH(a,b,level) ((a) == (b) ? (level) : (level+1))
#define LENS    200

#define DEBUG   0   // 0

// use this to read part of the line
#define SHORT_RUN

#include <boost/iostreams/device/file.hpp>
#include <boost/iostreams/filtering_stream.hpp>
#include <boost/iostreams/filter/gzip.hpp>

#include <armadillo>

using namespace arma;

vector<vector<string>>& ReadTSV(const char* filename)
{
    using namespace std;

    ifstream infile(filename);
    if (!infile) {
        cout << "unable to load file" << endl;
    }
    string str;

    vector<vector<string>> vvStr;

    vector<string> vecTitle;
    int pos1, pos2;

    // title

    getline(infile, str);
    pos1 = 0;
    while((pos2 = str.find('\t',pos1))!= string::npos)
    {
        vecTitle.push_back(str.substr(pos1, pos2-pos1));
        pos1 = ++pos2;
    }

    // values
    while (getline(infile, str))
    {
        pos1 = 0;
        vector<string> vStr;
        while((pos2 = str.find('\t',pos1))!= string::npos)
        {
            vStr.push_back(str.substr(pos1, pos2-pos1));
            pos1 = ++pos2;
        }
        vStr.push_back(str.substr(pos1, string::npos));
        vvStr.push_back(vStr);
    }

    return vvStr;
}

int gmm_test()
{
    // create synthetic data containing
    // 2 clusters with normal distribution
    uword d = 1; // dimensionality
    uword N = 10000; // number of samples (vectors)
    mat data(d, N, fill::zeros);
    vec mean1 = linspace<vec>(1,d,d);
    vec mean2 = mean1 + 5;
    uword i = 0;
    while(i < N)
    {
        if(i < N) { data.col(i) = mean1 + randn<vec>(d); ++i; }
        if(i < N) { data.col(i) = mean1 + randn<vec>(d); ++i; }
        if(i < N) { data.col(i) = mean2 + randn<vec>(d); ++i; }
    }
    // model the data as a diagonal GMM with 2 Gaussians
    gmm_diag model;
    bool status = model.learn(data, 2, maha_dist, random_subset,10, 5, 1e-10, true);
    if(status == false) { cout << "learning failed" << endl; }

    model.means.print("means:");

    double overall_likelihood = model.avg_log_p(data);

    rowvec set_likelihood = model.log_p( data.cols(0,9) );
    double scalar_likelihood = model.log_p( data.col(0) );

    uword gaus_id = model.assign( data.col(0), eucl_dist );
    urowvec gaus_ids = model.assign( data.cols(0,9), prob_dist );

    urowvec histogram1 = model.raw_hist (data, prob_dist);
    rowvec histogram2 = model.norm_hist(data, eucl_dist);

    cout << "histogram2\t" << histogram2[0] << "\t" << histogram2[1] << endl;

    model.save("/Users/jyang/my_model.gmm");

    mat modified_dcovs = 2 * model.dcovs;

    model.set_dcovs(modified_dcovs);

    return 0;
}

vector<Record> read_fasta( char* fa_filename )
{
    gmm_test();
    /*
   GaussianMixture aGmm = GaussianMixture(4,4,0.1,0.1);
   //aGmm.setValue();

   Eigen::VectorXf ax;
   ax << 1,2,3,4;

   Eigen::VectorXf bx;
    bx << 2,3,4,5;

    aGmm.setValue(ax,bx);

    //aGmm.value();
    */
   vector<Record> records;

   ifstream fin( fa_filename );
   if(!fin)
   {
      cerr << "Couldn't open the input file!";
      return records;
   }
   string line;
   size_t counter = -1;

   getline(fin, line);
   while(fin)
   {
      if(line[0] == '>')
      {
         records.push_back(Record(&line[1]));
         counter++;
      }
      else
      {
         records[counter].field += line;
      }
      getline(fin, line);
   }

   return records;
}

string reverse_complement( string DNA ){
    string output;
    for( int i = DNA.size()-1; i >= 0; i -- ){
        switch ( DNA[i] ){
            case 'a':
            case 'A':
                output += 'T';
                break;
            case 'c':
            case 'C':
                output += 'G';
                break;
            case 'g':
            case 'G':
                output += 'C';
                break;
            case 't':
            case 'T':
                output += 'A';
                break;
            default:
                output += DNA[i];
                break;
        }
    }
    return output;
}

/*
 *    Building fake tree structure based on the last 60 seq. from fasta (reverse complement).
 *    DWAG starting pointer should be given.
 *
 *    infilename : fasta file
 */
dwag* build_tree_from_records(dwag* start, vector<Record>& records ){
    for ( int i = 0; i < records.size(); i++ ){
        string rc_seq = reverse_complement( records[i].field );
        //cout << records[i].header << endl << records[i].field << endl << rc_seq << endl;

        dwag* next = start;
        //cout << start->name << endl;
        for ( int j = 0; j < 100; j++ ){
            char aChar = rc_seq[j];
            //cout << aChar;

            switch( aChar ){
                case 'A':
                    if ( next->A == NULL ){
                        next->A = new dwag;
                        next->A->name = "A";
                        next->A->level = next->level + 1;
                        next->A->parent = next;
                        next = next->A;
                    }else{
                        next = next->A;
                    }
                break;
                case 'C':
                    if ( next->C == NULL ){
                        next->C = new dwag;
                        next->C->name = "C";
                        next->C->level = next->level + 1;
                        next->C->parent = next;
                        next = next->C;
                    }else{
                        next = next->C;
                    }
                break;
                case 'G':
                    if ( next->G == NULL ){
                        next->G = new dwag;
                        next->G->name = "G";
                        next->G->level = next->level + 1;
                        next->G->parent = next;
                        next = next->G;
                    }else{
                        next = next->G;
                    }
                break;
                case 'T':
                    if ( next->T == NULL ){
                        next->T = new dwag;
                        next->T->name = "T";
                        next->T->level = next->level + 1;
                        next->T->parent = next;
                        next = next->T;
                    }else{
                        next = next->T;
                    }
                break;
            }
            if ( next->index == -2 ) next->index = i; // assign gene index when it is first time
            else next->index = -1; // overlap with other gene
        }
        //cout << endl;
        //cout << get_sequence( next ) << endl;
    }
    //cout << "$$$$$$ " << start->name << endl;
    return start;
}

dwag* mismatch_search_tree_c( dwag* next, char* value, int level, int mismatch, int* global_mismatch, dwag* found ){
    if ( mismatch >= MIS_MATCH_CUT ){
        return found;
    }
    if (level >= MIN_LEVEL_CUT){
        if ( mismatch < *global_mismatch ){
            *global_mismatch = mismatch;
            found = next;
            //cout << "REF: " << get_sequence( found ) << endl;
            //cout << "QUR: " << value << endl;
        }
        return found;
    }

    if ( next->A != NULL ) found = mismatch_search_tree_c( next->A, value, level+1, MATCH(value[level],'A',mismatch), global_mismatch, found );
    if ( next->C != NULL ) found = mismatch_search_tree_c( next->C, value, level+1, MATCH(value[level],'C',mismatch), global_mismatch, found );
    if ( next->G != NULL ) found = mismatch_search_tree_c( next->G, value, level+1, MATCH(value[level],'G',mismatch), global_mismatch, found );
    if ( next->T != NULL ) found = mismatch_search_tree_c( next->T, value, level+1, MATCH(value[level],'T',mismatch), global_mismatch, found );

    return found;
}

/*
 *
 * This used whole fastq files
 *
 */
namespace io = boost::iostreams;

int pair_search_with_tree_normal(dwag* start, map< pair<int, int>, int >& output, char *fastq1, char *fastq2, bool bShortRun = false, int readlength = 151){
    //cout << "### " << start->name << endl;
    char buffer1[LENS];
    for (int xx = 0; xx<LENS; xx++) buffer1[xx] = NULL;
    char buffer2[LENS];
    for (int xx = 0; xx<LENS; xx++) buffer2[xx] = NULL;
    char gz1[LENS];
    char gz2[LENS];
    std::cout << fastq1 << std::endl;
    std::cout << fastq2 << std::endl;

    gzFile infile1 = gzopen(fastq1, "rb");
    if (!infile1) return -1;

    gzFile infile2 = gzopen(fastq2, "rb");
    if (!infile2) return -1;

    char buffer[LENS];
    int count = 0;
    int found_cnt1 = 0;
    int found_cnt2 = 0;
    int global_mismatch = 1000;
    int found_both_cnt = 0;
    dwag* found1 = NULL;
    dwag* found2 = NULL;

    int i = 0;
    while( 0 != gzgets(infile1, buffer1, LENS) ){
        if ( 0 == gzgets(infile2, buffer2, LENS) ){
            break;
        }

        i++;
        if ( i % 100000 == 0 ) {
            cout << count << "\t" << found_cnt1 << "\t" << found_cnt2 << "\t" << found_both_cnt << "\t" << found_both_cnt / (float) count * 100.0 << endl;
            if (  i > 1000000 && bShortRun == true ) break;
        }

        if ( i % 4 != 2 ) continue;
        count += 1;
        global_mismatch = 1000;

        found1 = NULL;
        if ( found1 == NULL ) {
            for ( int xx = 30; xx < 60; xx ++ ){
                found1 = mismatch_search_tree_c( start, &buffer1[xx], 0, 0, &global_mismatch, found1 ); // for read1
                if ( found1 != NULL ) break;
            }
        }
        if ( found1 != NULL ){
            found_cnt1 += 1;
        }

        global_mismatch = 1000;
        found2 = NULL;
        if ( found2 == NULL ) {
            for ( int xx = 30; xx < 60; xx ++ ){
                found2 = mismatch_search_tree_c( start, &buffer2[xx], 0, 0, &global_mismatch, found2 ); // for read1
                 if ( found2 != NULL ) break;
            }
        }
        if ( found2 != NULL ){
            /*if ( global_mismatch == MIS_MATCH_CUT-1 ){
                cout << "READ2: " << i << "\t" << global_mismatch << "\t" << found2->index << endl;
                cout << "REF: " << get_sequence( found2 ) << endl;
                cout << "QUR: " << string( &buffer2[54] ) << endl;
            }*/
            found_cnt2 += 1;
        }
        if ( found1 != NULL && found2 != NULL ){
            ++output[pair<int, int>(found1->index, found2->index)];
            found_both_cnt ++;
        }
    }
    cout << "Total: " << count << endl;
    cout << "Found 1: " << found_cnt1 << "\t" << found_cnt1 / (float) count * 100.0 << endl;
    cout << "Found 2: " << found_cnt2 << "\t" << found_cnt2 / (float) count * 100.0 << endl;
    cout << "Found Both: " << found_both_cnt << "\t" << found_both_cnt / (float) count * 100.0 << endl;
    gzclose( infile1 );
    gzclose( infile2 );

/*
    io::filtering_istream in;
    in.push(io::gzip_decompressor());
    in.push(io::file_source(fastq1));
    // read from in using std::istream interface

    std::string line;
    while (std::getline(in, line, '\n'))
    {
         std::cout << line ; // your code :)
    }
*/

    return 0;
}

/*
 *
 * This used only second line - seq files
 *
 */
int pair_search_with_tree(dwag* start, map< pair<int, int>, int >& output, char *fastq1, char *fastq2, bool bShortRun = false, int readlength = 151){
    //cout << "### " << start->name << endl;
    char buffer1[LENS];
    for (int xx = 0; xx<LENS; xx++) buffer1[xx] = NULL;
    char buffer2[LENS];
    for (int xx = 0; xx<LENS; xx++) buffer2[xx] = NULL;
    char gz1[LENS];
    char gz2[LENS];
    sprintf( gz1, "%s.2", fastq1 );
    sprintf( gz2, "%s.2", fastq2 );
    std::cout << gz1 << std::endl;
    std::cout << gz2 << std::endl;

    int size = 0;
    int i = 0;
    int line = 0;
    int global_mismatch = 1000;
    int found_cnt1 = 0;
    int found_cnt2 = 0;
    int found_both_cnt = 0;
    dwag* found1 = NULL;
    dwag* found2 = NULL;

    gzFile infile1 = gzopen(gz1, "rb");
    if (!infile1) return -1;
    gzFile infile2 = gzopen(gz2, "rb");
    if (!infile2) return -1;

    while( 0 != (size = gzread(infile1, buffer1, readlength+1) ) ){
        (size = gzread(infile2, buffer2, readlength+1) );
        //cout << buffer1 << buffer2 << endl;
        global_mismatch = 1000;
        i++;
        if ( i % 100000 == 0 ) {
            cout << i << "\t" << found_cnt1 << "\t" << found_cnt2 << "\t" << found_both_cnt << "\t" << found_both_cnt / (float) i << endl;
            if (  i > 1000000 && bShortRun == true ) break;
        }
        found1 = NULL;
        //found1 = mismatch_search_tree_c( start, &buffer1[52], 0, 0, &global_mismatch, found1 ); // for read1
        //if ( found1 == NULL ) found1 = mismatch_search_tree_c( start, &buffer1[50], 0, 0, &global_mismatch, found1 ); // for read1
        //if ( found1 == NULL ) found1 = mismatch_search_tree_c( start, &buffer1[51], 0, 0, &global_mismatch, found1 ); // for read1
        //if ( found1 == NULL ) found1 = mismatch_search_tree_c( start, &buffer1[53], 0, 0, &global_mismatch, found1 ); // for read1
        //if ( found1 == NULL ) found1 = mismatch_search_tree_c( start, &buffer1[54], 0, 0, &global_mismatch, found1 ); // for read1
        if ( found1 == NULL ) {
            for ( int xx = 0; xx < 100; xx ++ ){
                found1 = mismatch_search_tree_c( start, &buffer1[xx], 0, 0, &global_mismatch, found1 ); // for read1
                if ( found1 != NULL ) break;
            }
        }
        if ( found1 != NULL ){
            /*if ( global_mismatch == MIS_MATCH_CUT-1 ){
                cout << "READ1: " << i << "\t" << global_mismatch << "\t" << found1->index << endl;
                cout << "REF: " << get_sequence( found1 ) << endl;
                cout << "QUR: " << string( &buffer1[52] ) << endl;
            }*/
            found_cnt1 += 1;
        }
        global_mismatch = 1000;
        found2 = NULL;
        //found2 = mismatch_search_tree_c( start, &buffer2[54], 0, 0, &global_mismatch, found2 ); // for read2
        //if ( found2 == NULL ) found2 = mismatch_search_tree_c( start, &buffer2[52], 0, 0, &global_mismatch, found2 ); // for read2
        //if ( found2 == NULL ) found2 = mismatch_search_tree_c( start, &buffer2[53], 0, 0, &global_mismatch, found2 ); // for read2
        //if ( found2 == NULL ) found2 = mismatch_search_tree_c( start, &buffer2[55], 0, 0, &global_mismatch, found2 ); // for read2
        //if ( found2 == NULL ) found2 = mismatch_search_tree_c( start, &buffer2[56], 0, 0, &global_mismatch, found2 ); // for read2
        if ( found2 == NULL ) {
            for ( int xx = 0; xx < 100; xx ++ ){
                found2 = mismatch_search_tree_c( start, &buffer2[xx], 0, 0, &global_mismatch, found2 ); // for read1
                 if ( found2 != NULL ) break;
            }
        }
        if ( found2 != NULL ){
            /*if ( global_mismatch == MIS_MATCH_CUT-1 ){
                cout << "READ2: " << i << "\t" << global_mismatch << "\t" << found2->index << endl;
                cout << "REF: " << get_sequence( found2 ) << endl;
                cout << "QUR: " << string( &buffer2[54] ) << endl;
            }*/
            found_cnt2 += 1;
        }
        if ( found1 != NULL && found2 != NULL ){
            ++output[pair<int, int>(found1->index, found2->index)];
            found_both_cnt ++;
        }
    }
    cout << "Found 1: " << found_cnt1 << "\t" << found_cnt1 / (float) i << endl;
    cout << "Found 2: " << found_cnt2 << "\t" << found_cnt2 / (float) i << endl;
    cout << "Found Both: " << found_both_cnt << "\t" << found_both_cnt / (float) i << endl;
    gzclose( infile1 );
    gzclose( infile2 );
    return 0;
}

void print_map_pair_values( map<pair<int, int>,int>& output ){
    for ( map< pair<int,int>, int>::iterator it = output.begin(); it != output.end();  it++ ){
        cout << it->first.first << "," << it->first.second << "\t" << it->second << endl;
    }
}

void print_map_pair_map( map<pair<int, int>,int>& output, vector<Record>& records ){
    int total_cnt = 0;
    int zero_cnt = 0;
    for ( int i = 0; i < records.size(); i ++ ){
        for ( int j = 0; j < records.size(); j ++ ){
            int pair_cnt = output[ pair<int,int>(i,j) ];
            cout << i << "," << j << "\t" << records[i].header << "," << records[j].header << "\t" << pair_cnt << endl;
            if ( pair_cnt == 0 ) zero_cnt ++;
            total_cnt ++;
        }
    }
    cout << "Total: " << total_cnt << "\tZeros: " << zero_cnt << "\tFound: " << (total_cnt-zero_cnt) / (float)(total_cnt) * 100.0 << "%" << endl;
    /*for ( map< pair<int,int>, int>::iterator it = output.begin(); it != output.end();  it++ ){
        cout << it->first.first << "," << it->first.second << "\t" << it->second << endl;
    }*/
}

void write_comment_conditions( char * outfilename ){
    ofstream outFile( outfilename );
    outFile << "# ALLOWED_MISMATCH " << MIS_MATCH_CUT - 1 << endl;
    outFile << "# MIN_MATCH_LENGTH " << MIN_LEVEL_CUT << endl;
    outFile.close();
}

void write_map_pair_map( map<pair<int, int>,int>& output, vector<Record>& records, char * outfilename ){
    ofstream outFile;
    outFile.open( outfilename, ofstream::out | ofstream::app );

    int total_cnt = 0;
    int zero_cnt = 0;
    for ( int i = 0; i < records.size(); i ++ )
        outFile << "\t" << records[i].header;
    outFile << endl;
    for ( int i = 0; i < records.size(); i ++ ){
        outFile << records[i].header;
        for ( int j = 0; j < records.size(); j ++ ){
            int pair_cnt = output[ pair<int,int>(i,j) ];
            outFile << "\t" << pair_cnt;
            //outFile << i << "," << j << "\t" << records[i].header << "," << records[j].header << "\t" << pair_cnt << endl;
            if ( pair_cnt == 0 ) zero_cnt ++;
            total_cnt ++;
        }
        outFile << endl;
    }
    //outFile << "Total: " << total_cnt << "\tZeros: " << zero_cnt << "\tFound: " << (total_cnt-zero_cnt) / (float)(total_cnt) * 100.0 << "%" << endl;
    outFile.close();
    /*for ( map< pair<int,int>, int>::iterator it = output.begin(); it != output.end();  it++ ){
 *         cout << it->first.first << "," << it->first.second << "\t" << it->second << endl;
 *             }*/
}

/*
 * argv[1] : fasta
 * argv[2] : fastq1
 * argv[3] : fastq2
 */
int fusion_pair_search(int argc, char *argv[]){
    //std::cout << "fusion_pair_search" << std::endl;

    dwag start;
    map<pair<int, int>,int> output;

    //cout << "read_fasta: " << argv[2] << endl;
    vector<Record> records = read_fasta( argv[2] );

    //cout << "build_tree_from_records" << endl;
    build_tree_from_records( &start, records );

    //cout << "pair_search_with_tree: " << argv[3] << " , " << argv[4] << endl;
    pair_search_with_tree( &start, output, argv[3], argv[4], 151 );

    print_map_pair_values( output );
    print_map_pair_map( output, records );
    write_comment_conditions( argv[5] );
    write_map_pair_map( output, records, argv[5] );
    return 0;
}


// Copyright (c) 2015-2019 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#ifndef threshold_filter_hpp
#define threshold_filter_hpp

#include <vector>
#include <string>
#include <functional>

#include <boost/optional.hpp>
#include <boost/variant.hpp>
#include <boost/any.hpp>

#include "single_pass_variant_call_filter.hpp"
#include "logging/progress_meter.hpp"
#include "../facets/facet_factory.hpp"

namespace octopus {

class VcfHeader;

namespace csr {

class ThresholdVariantCallFilter : public SinglePassVariantCallFilter
{
public:
    struct Threshold
    {
        virtual ~Threshold() = default;
        virtual std::unique_ptr<Threshold> clone() const = 0;
        virtual bool operator()(Measure::ResultType value) const noexcept = 0;
    };

    struct ThresholdWrapper
    {
        ThresholdWrapper(std::unique_ptr<Threshold> threshold) : threshold {std::move(threshold)} {}
        ThresholdWrapper(const ThresholdWrapper& other) : threshold {other.threshold->clone()} {}
        ThresholdWrapper& operator=(const ThresholdWrapper& other)
        {
            if (&other != this) threshold = other.threshold->clone();
            return *this;
        }
        bool operator()(Measure::ResultType value) const noexcept { return (*threshold)(value); }
        std::unique_ptr<Threshold> threshold;
    };

    struct Condition
    {
        MeasureWrapper measure;
        ThresholdWrapper threshold;
        std::string vcf_filter_key = ".";
    };

    struct ConditionVectorPair
    {
        std::vector<Condition> hard, soft;
    };

    ThresholdVariantCallFilter() = delete;

    ThresholdVariantCallFilter(FacetFactory facet_factory,
                               ConditionVectorPair conditions,
                               OutputOptions output_config,
                               ConcurrencyPolicy threading,
                               boost::optional<ProgressMeter&> progress = boost::none,
                               std::vector<MeasureWrapper> other_measures = {});

    ThresholdVariantCallFilter(const ThresholdVariantCallFilter&)            = delete;
    ThresholdVariantCallFilter& operator=(const ThresholdVariantCallFilter&) = delete;
    ThresholdVariantCallFilter(ThresholdVariantCallFilter&&)                 = delete;
    ThresholdVariantCallFilter& operator=(ThresholdVariantCallFilter&&)      = delete;

    virtual ~ThresholdVariantCallFilter() override = default;

protected:
    using ThresholdVector   = std::vector<ThresholdWrapper>;
    using ThresholdIterator = ThresholdVector::const_iterator;
    using MeasureIterator   = MeasureVector::const_iterator;

    ThresholdVector hard_thresholds_, soft_thresholds_;
    std::vector<std::string> vcf_filter_keys_;
    bool all_unique_filter_keys_;

    bool passes_all_filters(MeasureIterator first_measure, MeasureIterator last_measure,
                            ThresholdIterator first_threshold) const;

private:
    std::string do_name() const override;
    virtual void annotate(VcfHeader::Builder& header) const override;
    virtual Classification classify(const MeasureVector& measures) const override;

    virtual bool passes_all_hard_filters(const MeasureVector& measures) const;
    virtual bool passes_all_soft_filters(const MeasureVector& measures) const;
    virtual std::vector<std::string> get_failing_vcf_filter_keys(const MeasureVector& measures) const;
};

template <typename M, typename... Args>
decltype(auto) make_wrapped_threshold(Args&&... args)
{
    using Wrapper = ThresholdVariantCallFilter::ThresholdWrapper;
    return Wrapper {std::make_unique<M>(std::forward<Args>(args)...)};
}

namespace detail {

template <typename Cmp, typename T = double>
struct UnaryThreshold
{
    explicit UnaryThreshold(T target, Cmp cmp) : visitor_ {target, cmp} {}
    bool operator()(Measure::ResultType value) const noexcept
    {
        return boost::apply_visitor(visitor_, value);
    }
private:
    struct UnaryVisitor : public boost::static_visitor<bool>
    {
        explicit UnaryVisitor(T target, Cmp cmp) : target {target}, cmp {cmp} {}
        template <typename T_>
        bool operator()(T_ value) const noexcept { return !cmp(value, target); }
        template <typename T_>
        bool operator()(boost::optional<T_> value) const noexcept
        {
            return !value || (*this)(*value);
        }
        bool operator()(boost::any value) const noexcept
        {
            return true;
        }
        template <typename T_>
        bool operator()(const std::vector<T_>& values) const noexcept
        {
            return std::all_of(std::cbegin(values), std::cend(values), [this] (const auto& value) { return (*this)(value); });
        }
        T target;
        Cmp cmp;
    };
    UnaryVisitor visitor_;
};

} // namespace detail

template <typename T = double>
struct EqualThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit EqualThreshold(T target) : base_ {target, std::equal_to<> {}} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<EqualThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return base_(value);
    }
private:
    detail::UnaryThreshold<std::equal_to<>, T> base_;
};

template <typename T = double>
struct NotEqualThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit NotEqualThreshold(T target) : base_ {target, std::not_equal_to<> {}} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<NotEqualThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return base_(value);
    }
private:
    detail::UnaryThreshold<std::not_equal_to<>, T> base_;
};

template <typename T = double>
struct LessThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit LessThreshold(T target) : base_ {target, std::less<> {}} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<LessThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return base_(value);
    }
private:
    detail::UnaryThreshold<std::less<>, T> base_;
};

template <typename T = double>
struct LessEqualThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit LessEqualThreshold(T target) : base_ {target, std::less_equal<> {}} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<LessEqualThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return base_(value);
    }
private:
    detail::UnaryThreshold<std::less_equal<>, T> base_;
};

template <typename T = double>
struct GreaterThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit GreaterThreshold(T target) : base_ {target, std::greater<> {}} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<GreaterThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return base_(value);
    }
private:
    detail::UnaryThreshold<std::greater<>, T> base_;
};

template <typename T = double>
struct GreaterEqualThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit GreaterEqualThreshold(T target) : base_ {target, std::greater_equal<> {}} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<GreaterEqualThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return base_(value);
    }
private:
    detail::UnaryThreshold<std::greater_equal<>, T> base_;
};

template <typename T = double>
struct BetweenThreshold : public ThresholdVariantCallFilter::Threshold
{
    explicit BetweenThreshold(T lower_bound, T upper_bound) : visitor_ {lower_bound, upper_bound} {}
    std::unique_ptr<ThresholdVariantCallFilter::Threshold> clone() const
    {
        return std::make_unique<BetweenThreshold>(*this);
    }
    bool operator()(Measure::ResultType value) const noexcept
    {
        return boost::apply_visitor(visitor_, value);
    }
private:
    struct BetweenVisitor : public boost::static_visitor<bool>
    {
        explicit BetweenVisitor(T lower_bound, T upper_bound)
        : lower_bound {lower_bound}, upper_bound {upper_bound} {}
        template <typename T_>
        bool operator()(T_ value) const noexcept
        {
            return lower_bound <= value && value <= upper_bound;
        }
        template <typename T_>
        bool operator()(boost::optional<T_> value) const noexcept
        {
            return !value || (*this)(*value);
        }
        T lower_bound, upper_bound;
    };
    BetweenVisitor visitor_;
};

} // namespace csr
} // namespace octopus

#endif


#include <boost/python.hpp>
#include <string>
#include "inf_def_visitor.hxx"

#include <opengm/inference/graphcut.hxx>
#include <opengm/inference/alphabetaswap.hxx>
#include <opengm/inference/alphaexpansion.hxx>
#ifdef WITH_BOOST
#  include <opengm/inference/auxiliary/minstcutboost.hxx>
#endif
#ifdef WITH_MAXFLOW
#  include <opengm/inference/auxiliary/minstcutkolmogorov.hxx>
#endif
#ifdef WITH_MAXFLOW_IBFS
#  include <opengm/inference/auxiliary/minstcutibfs.hxx>
#endif
# include <param/graph_cut_param.hxx>

#include <opengm/python/opengmpython.hxx>
#include <opengm/python/converter.hxx>
#include <opengm/python/numpyview.hxx>
#include <opengm/python/pythonfunction.hxx>

using namespace boost::python;

template<class GM,class ACC>
void export_graphcut(){
   import_array();
   typedef GM PyGm;
   typedef typename PyGm::ValueType ValueType;
   typedef typename PyGm::IndexType IndexType;

   append_subnamespace("solver");

   #ifdef WITH_MAXFLOW
      const bool withMaxFlow=true;
   #else
      const bool withMaxFlow=false;
   #endif

   #ifdef WITH_MAXFLOW_IBFS
      const bool withMaxFlowIbfs=true;
   #else
      const bool withMaxFlowIbfs=false;
   #endif

   // documentation
   InfSetup setup;
   setup.cite       = "";
   setup.algType    = "graphCut";
   setup.guarantees = "optimal ";
   setup.limitations= "max 2.order, binary labels, must be submodular";
   setup.hyperParameterKeyWords = StringVector(1,std::string("minStCut"));
   setup.hyperParametersDoc     = StringVector(1,std::string("minStCut implementation of graphcut"));
   setup.dependencies = "to use ``'kolmogorov'`` as minStCut the kolmogorov max flow library, "
                        "compile OpenGM with CMake-Flag ``WITH_CPLEX`` set to ``ON`` ";

   #ifdef WITH_MAXFLOW
      // set up hyper parameter name for this template
      setup.isDefault=withMaxFlow;
      setup.hyperParameters= StringVector(1,std::string("kolmogorov"));
      typedef opengm::external::MinSTCutKolmogorov<size_t,ValueType> MinStCutKolmogorov;
      typedef opengm::GraphCut<PyGm, ACC, MinStCutKolmogorov>        PyGraphCutKolmogorov;
      // export parameter
      exportInfParam<PyGraphCutKolmogorov>("_GraphCut_Kolmogorov");
      // export inference
      class_< PyGraphCutKolmogorov>("_GraphCut_Kolmogorov",init<const GM & >())
      .def(InfSuite<PyGraphCutKolmogorov,false,true,true>(std::string("GraphCut"),setup))
      ;
   #endif

   #ifdef WITH_MAXFLOW_IBFS
      // set up hyper parameter name for this template
      setup.isDefault=!withMaxFlow;
      setup.hyperParameters= StringVector(1,std::string("ibfs"));
      typedef opengm::external::MinSTCutIBFS<size_t,ValueType> MinStCutType;
      typedef opengm::GraphCut<PyGm, ACC, MinStCutType>        PyGraphCutIbfs;
      // export parameter
      exportInfParam<PyGraphCutIbfs>("_GraphCut_Ibfs");
      // export inference
      class_< PyGraphCutIbfs>("_GraphCut_Ibfs",init<const GM & >())
      .def(InfSuite<PyGraphCutIbfs,false,true,false>(std::string("GraphCut"),setup))
      ;
   #endif

   // set up hyper parameter name for this template
   setup.isDefault=(!withMaxFlow) && (!withMaxFlowIbfs);
   setup.hyperParameters= StringVector(1,std::string("boost-kolmogorov"));
   typedef opengm::MinSTCutBoost<size_t, ValueType, opengm::KOLMOGOROV> MinStCutBoostKolmogorov;
   typedef opengm::GraphCut<PyGm, ACC, MinStCutBoostKolmogorov> PyGraphCutBoostKolmogorov;
   // export parameter
   exportInfParam<PyGraphCutBoostKolmogorov>("_GraphCut_Boost_Kolmogorov");
   // export inference
   class_< PyGraphCutBoostKolmogorov>("_GraphCut_Boost_Kolmogorov",init<const GM & >())
   .def(InfSuite<PyGraphCutBoostKolmogorov,false,true,false>(std::string("GraphCut"),setup))
   ;

   // set up hyper parameter name for this template
   setup.isDefault=false;
   setup.hyperParameters= StringVector(1,std::string("push-relabel"));
   typedef opengm::MinSTCutBoost<size_t, ValueType, opengm::PUSH_RELABEL> MinStCutBoostPushRelabel;
   typedef opengm::GraphCut<PyGm, ACC, MinStCutBoostPushRelabel> PyGraphCutBoostPushRelabel;
   // export parameter
   exportInfParam<PyGraphCutBoostPushRelabel>("_GraphCut_Boost_Push_Relabel");
   // export inference
   class_< PyGraphCutBoostPushRelabel>("_GraphCut_Boost_Push_Relabel",init<const GM & >())
   .def(InfSuite<PyGraphCutBoostPushRelabel,false,true,false>(std::string("GraphCut"),setup))
   ;

}

template void export_graphcut<opengm::python::GmAdder,opengm::Minimizer>();


// Copyright (c) 2020 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <algorithm>
#include <cmath>
#include <limits>
#include <stdexcept>
#include "pyinterp/detail/math.hpp"

namespace pyinterp::detail::axis::container {

/// Abstraction of a container of values representing a mathematical axis.
///
/// @tparam T type of data handled by this container
template <typename T>
class Abstract {
 public:
  /// Default constructor
  Abstract() = default;

  /// Default destructor
  virtual ~Abstract() = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Abstract(const Abstract& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Abstract(Abstract&& rhs) = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Abstract& rhs) -> Abstract& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Abstract&& rhs) -> Abstract& = default;

  /// Returns true if the data is arranged in ascending order.
  [[nodiscard]] inline auto is_ascending() const -> bool {
    return is_ascending_;
  }

  /// Reverse the order of elements in this axis
  virtual auto flip() -> void = 0;

  /// Checks thats axis is monotonic
  [[nodiscard]] virtual auto is_monotonic() const -> bool { return true; }

  /// Get the ith coordinate value.
  ///
  /// @param index which coordinate. Between 0 and size()-1 inclusive
  /// @return coordinate value
  [[nodiscard]] virtual auto coordinate_value(size_t index) const -> T = 0;

  /// Get the minimum coordinate value.
  ///
  /// @return minimum coordinate value
  [[nodiscard]] virtual auto min_value() const -> T = 0;

  /// Get the maximum coordinate value.
  ///
  /// @return maximum coordinate value
  [[nodiscard]] virtual auto max_value() const -> T = 0;

  /// Get the number of values for this axis
  ///
  /// @return the number of values
  [[nodiscard]] virtual auto size() const -> int64_t = 0;

  /// Gets the first element in the container
  ///
  /// @return the first element
  [[nodiscard]] virtual auto front() const -> T = 0;

  /// Gets the last element in the container
  ///
  /// @return the last element
  [[nodiscard]] virtual auto back() const -> T = 0;

  /// Search for the index corresponding to the requested value.
  ///
  /// @param coordinate position in this coordinate system
  /// @param bounded if true, returns "-1" if the value is located outside this
  /// coordinate system, otherwise the value of the first element if the value
  /// is located before, or the value of the last element of this container if
  /// the requested value is located after.
  /// @return index of the requested value it or -1 if outside this coordinate
  /// system area.
  [[nodiscard]] virtual auto find_index(T coordinate, bool bounded) const
      -> int64_t = 0;

  /// compare two variables instances
  ///
  /// @param rhs A variable to compare
  /// @return if variables are equals
  virtual auto operator==(const Abstract& rhs) const -> bool = 0;

 protected:
  /// Indicates whether the data is stored in the ascending order.
  bool is_ascending_{true};

  /// Calculate if the data is arranged in ascending order.
  [[nodiscard]] inline auto calculate_is_ascending() const -> bool {
    return size() < 2 ? true : coordinate_value(0) < coordinate_value(1);
  }
};

/// Represents a container for an undefined axis
///
/// @tparam T type of data handled by this container
template <typename T>
class Undefined : public Abstract<T> {
 public:
  /// Default constructor
  Undefined() = default;

  /// Default destructor
  ~Undefined() override = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Undefined(const Undefined& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Undefined(Undefined&& rhs) = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Undefined& rhs) -> Undefined& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Undefined&& rhs) -> Undefined& = default;

  /// @copydoc Abstract::flip()
  auto flip() -> void override {}

  /// @copydoc Abstract::coordinate_value(const size_t) const
  [[nodiscard]] inline auto coordinate_value(const size_t /* index */) const
      noexcept -> T override {
    return math::Fill<T>::value();
  }

  /// @copydoc Abstract::min_value() const
  [[nodiscard]] inline auto min_value() const noexcept -> T override {
    return coordinate_value(0);
  }

  /// @copydoc Abstract::max_value() const
  [[nodiscard]] inline auto max_value() const noexcept -> T override {
    return coordinate_value(0);
  }

  /// @copydoc Abstract::size() const
  [[nodiscard]] inline auto size() const noexcept -> int64_t override {
    return 0;
  }

  /// @copydoc Abstract::front() const
  [[nodiscard]] inline auto front() const noexcept -> T override {
    return coordinate_value(0);
  }

  /// @copydoc Abstract::back() const
  [[nodiscard]] inline auto back() const noexcept -> T override {
    return coordinate_value(0);
  }

  /// @copydoc Abstract::find_index(double,bool) const
  inline int64_t find_index(T /* coordinate */,
                            bool /* bounded */) const  /// NOLINT
      noexcept override {
    return -1;
  }

  /// @copydoc Abstract::operator==(const Abstract&) const
  inline auto operator==(const Abstract<T>& rhs) const noexcept
      -> bool override {
    return dynamic_cast<const Undefined<T>*>(&rhs) != nullptr;
  }
};

/// Represents a container for an irregularly spaced axis
///
/// @tparam T type of data handled by this container
template <typename T>
class Irregular : public Abstract<T> {
 public:
  /// Creation of a container representing an irregularly spaced coordinate
  /// system.
  ///
  /// @param points axis values
  explicit Irregular(Eigen::Matrix<T, Eigen::Dynamic, 1> points)
      : points_(std::move(points)) {
    if (points_.size() == 0) {
      throw std::invalid_argument("unable to create an empty container.");
    }
    this->is_ascending_ = this->calculate_is_ascending();
    make_edges();
  }

  /// Destructor
  ~Irregular() override = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Irregular(const Irregular& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Irregular(Irregular&& rhs) = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Irregular& rhs) -> Irregular& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Irregular&& rhs) -> Irregular& = default;

  /// @copydoc Abstract::flip()
  auto flip() -> void override {
    std::reverse(points_.data(), points_.data() + points_.size());
    this->is_ascending_ = !this->is_ascending_;
    make_edges();
  }

  /// @copydoc Abstract::is_monotonic() const
  [[nodiscard]] inline auto is_monotonic() const noexcept -> bool override {
    if (this->is_ascending_) {
      return std::is_sorted(points_.data(), points_.data() + points_.size());
    }
    return std::is_sorted(points_.data(), points_.data() + points_.size(),
                          std::greater<>());
  };

  /// @copydoc Abstract::coordinate_value(const size_t) const
  [[nodiscard]] inline auto coordinate_value(const size_t index) const
      -> T override {
    return points_[index];
  }

  /// @copydoc Abstract::min_value() const
  [[nodiscard]] inline auto min_value() const -> T override {
    return this->is_ascending_ ? front() : back();
  }

  /// @copydoc Abstract::max_value() const
  [[nodiscard]] inline auto max_value() const -> T override {
    return this->is_ascending_ ? back() : front();
  }

  /// @copydoc Abstract::size() const
  [[nodiscard]] inline auto size() const noexcept -> int64_t override {
    return points_.size();
  }

  /// @copydoc Abstract::front() const
  [[nodiscard]] inline auto front() const -> T override { return points_[0]; }

  /// @copydoc Abstract::back() const
  [[nodiscard]] inline auto back() const -> T override {
    return points_[points_.size() - 1];
  }

  /// @copydoc Abstract::find_index(double,bool) const
  [[nodiscard]] auto find_index(T coordinate, bool bounded) const
      -> int64_t override {
    int64_t low = 0;
    int64_t mid = 0;
    int64_t high = size();

    if (this->is_ascending_) {
      if (coordinate < edges_[0]) {
        return bounded ? 0 : -1;
      }

      if (coordinate > edges_[edges_.size() - 1]) {
        return bounded ? high - 1 : -1;
      }

      while (high > low + 1) {
        // low and high are strictly positive
        mid = (low + high) >> 1;  // NOLINT
        auto value = edges_[mid];

        if (value == coordinate) {
          return mid;
        }
        value < coordinate ? low = mid : high = mid;
      }
      return low;
    }

    if (coordinate < edges_[edges_.size() - 1]) {
      return bounded ? high - 1 : -1;
    }

    if (coordinate > edges_[0]) {
      return bounded ? 0 : -1;
    }

    while (high > low + 1) {
      // low and high are strictly positive
      mid = (low + high) >> 1;  // NOLINT
      auto value = edges_[mid];

      if (value == coordinate) {
        return mid;
      }
      value < coordinate ? high = mid : low = mid;
    }
    return low;
  }

  /// @copydoc Abstract::operator==(const Abstract&) const
  auto operator==(const Abstract<T>& rhs) const noexcept -> bool override {
    const auto ptr = dynamic_cast<const Irregular<T>*>(&rhs);
    if (ptr != nullptr) {
      return ptr->points_.size() == points_.size() && ptr->points_ == points_;
    }
    return false;
  }

 private:
  Eigen::Matrix<T, Eigen::Dynamic, 1> points_{};
  Eigen::Matrix<T, Eigen::Dynamic, 1> edges_{};

  /// Computes the edges, if the axis data are not spaced regularly.
  void make_edges() {
    auto n = points_.size();
    edges_.resize(n + 1);

    for (Eigen::Index ix = 1; ix < n; ++ix) {
      edges_[ix] = (points_[ix - 1] + points_[ix]) / 2;
    }

    edges_[0] = 2 * points_[0] - edges_[1];
    edges_[n] = 2 * points_[n - 1] - edges_[n - 1];
  }
};

/// Represents a container for an regularly spaced axis
///
/// @tparam T type of data handled by this container
template <typename T>
class Regular : public Abstract<T> {
 public:
  /// Create a container from evenly spaced numbers over a specified
  /// interval.
  ///
  /// @param start the starting value of the sequence
  /// @param stop the end value of the sequence
  /// @param num number of samples in the container
  Regular(const T start, const T stop, const T num)
      : size_(static_cast<int64_t>(num)), start_(start) {
    if (num == 0) {
      throw std::invalid_argument("unable to create an empty container.");
    }
    step_ = num == 1 ? stop - start : (stop - start) / (num - 1);
    // The inverse step of this axis is stored in order to optimize the search
    // for an index for a given value by avoiding a division.
    inv_step_ = 1.0 / step_;
    this->is_ascending_ = this->calculate_is_ascending();
  }

  /// Destructor
  ~Regular() override = default;

  /// Copy constructor
  ///
  /// @param rhs right value
  Regular(const Regular& rhs) = default;

  /// Move constructor
  ///
  /// @param rhs right value
  Regular(Regular&& rhs) = default;

  /// Copy assignment operator
  ///
  /// @param rhs right value
  auto operator=(const Regular& rhs) -> Regular& = default;

  /// Move assignment operator
  ///
  /// @param rhs right value
  auto operator=(Regular&& rhs) -> Regular& = default;

  /// Get the step between two successive values.
  ///
  /// @return increment value
  [[nodiscard]] auto step() const -> T { return step_; }

  /// @copydoc Abstract::flip()
  auto flip() -> void override {
    start_ = back();
    step_ = -step_;
    inv_step_ = -inv_step_;
    this->is_ascending_ = !this->is_ascending_;
  }

  /// @copydoc Abstract::coordinate_value(const size_t) const
  [[nodiscard]] inline auto coordinate_value(const size_t index) const noexcept
      -> T override {
    return start_ + index * step_;
  }

  /// @copydoc Abstract::find_index(double,bool) const
  [[nodiscard]] auto find_index(T coordinate, bool bounded) const noexcept
      -> int64_t override {
    auto index =
        static_cast<int64_t>(std::round((coordinate - start_) * inv_step_));

    if (index < 0) {
      return bounded ? 0 : -1;
    }

    if (index >= size_) {
      return bounded ? size_ - 1 : -1;
    }
    return index;
  }

  /// @copydoc Abstract::min_value() const
  [[nodiscard]] inline auto min_value() const noexcept -> T override {
    return coordinate_value(this->is_ascending_ ? 0 : size_ - 1);
  }

  /// @copydoc Abstract::max_value() const
  [[nodiscard]] inline auto max_value() const noexcept -> T override {
    return coordinate_value(this->is_ascending_ ? size_ - 1 : 0);
  }

  /// @copydoc Abstract::front() const
  [[nodiscard]] inline auto front() const noexcept -> T override {
    return start_;
  }

  /// @copydoc Abstract::back() const
  [[nodiscard]] inline auto back() const noexcept -> T override {
    return coordinate_value(size_ - 1);
  }

  /// @copydoc Abstract::size() const
  [[nodiscard]] inline auto size() const noexcept -> int64_t override {
    return size_;
  }

  /// @copydoc Abstract::operator==(const Abstract&) const
  auto operator==(const Abstract<T>& rhs) const noexcept -> bool override {
    const auto ptr = dynamic_cast<const Regular<T>*>(&rhs);
    if (ptr != nullptr) {
      return ptr->step_ == step_ && ptr->start_ == start_ &&
             ptr->size_ == size_;
    }
    return false;
  }

 private:
  /// Container size.
  int64_t size_{};
  /// Value of the first item in the container.
  T start_{};
  /// The step between two succeeding values.
  T step_{};
  /// The inverse of the step (to avoid a division between real numbers).
  double inv_step_{};
};

}  // namespace pyinterp::detail::axis::container


//---------------------------------------------------------------------------//
// Copyright (c) 2018-2020 Mikhail Komarov <nemo@nil.foundation>
// Copyright (c) 2020 Nikita Kaskov <nbering@nil.foundation>
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//---------------------------------------------------------------------------//

#ifndef CRYPTO3_BLOCK_BLOCK_STATE_PREPROCESSOR_HPP
#define CRYPTO3_BLOCK_BLOCK_STATE_PREPROCESSOR_HPP

#include <array>
#include <iterator>
#include <climits>

#include <boost/crypto3/detail/pack.hpp>
#include <boost/crypto3/detail/digest.hpp>

#include <boost/crypto3/block/accumulators/bits_count.hpp>
#include <boost/crypto3/block/accumulators/parameters/bits.hpp>

#include <boost/integer.hpp>
#include <boost/cstdint.hpp>
#include <boost/static_assert.hpp>
#include <boost/utility/enable_if.hpp>

#include <boost/range/algorithm/copy.hpp>

namespace boost {
    namespace crypto3 {
        namespace block {
            template<typename Mode, typename StateAccumulator, typename Params>
            struct block_stream_processor {
            private:
                typedef Mode mode_type;
                typedef StateAccumulator accumulator_type;
                typedef Params params_type;

                typedef typename mode_type::block_type block_type;
                constexpr static const std::size_t block_bits = mode_type::block_bits;

                constexpr static const std::size_t word_bits = mode_type::word_bits;

                constexpr static const std::size_t actual_bits = sizeof(typename block_type::value_type) * CHAR_BIT;

            public:
                typedef typename mode_type::endian_type endian_type;

                constexpr static const std::size_t value_bits = params_type::value_bits;
                typedef typename boost::uint_t<value_bits>::least value_type;
                BOOST_STATIC_ASSERT(block_bits % value_bits == 0);
                constexpr static const std::size_t block_values = block_bits / value_bits;
                typedef std::array<value_type, block_values> cache_type;

            private:
                constexpr static const std::size_t length_bits = params_type::length_bits;
                // FIXME: do something more intelligent than capping at sizeof(boost::uintmax_t) * CHAR_BIT
                constexpr static const std::size_t length_type_bits = length_bits < word_bits ?
                                                                          word_bits :
                                                                          length_bits >
                                                                                sizeof(boost::uintmax_t) * CHAR_BIT ?
                                                                          sizeof(boost::uintmax_t) * CHAR_BIT :
                                                                          length_bits;
                typedef typename boost::uint_t<length_type_bits>::least length_type;

                BOOST_STATIC_ASSERT(!length_bits || length_bits % word_bits == 0);
                BOOST_STATIC_ASSERT(block_bits % value_bits == 0);

                BOOST_STATIC_ASSERT(!length_bits || value_bits <= length_bits);

                inline void process_block(std::size_t block_seen = block_bits) {
                    using namespace boost::crypto3::detail;
                    // Convert the input into words
                    block_type block;
                    pack_to<endian_type, value_bits, actual_bits>(cache.begin(), cache.end(), block.begin());
                    // Process the block
                    acc(block, accumulators::bits = block_seen);
                }

            public:
                inline void update_one(value_type value) {
                    cache[cache_seen] = value;
                    ++cache_seen;
                    if (cache_seen == block_values) {
                        // Process the completed block
                        process_block();
                        cache_seen = 0;
                    }
                }

                template<typename InputIterator>
                inline void update_n(InputIterator p, size_t n) {
                    for (; n; --n) {
                        update_one(*p++);
                    }
                }

                template<typename InputIterator>
                inline void update_n(InputIterator first, InputIterator last) {
                    std::size_t n = std::distance(first, last);
                    update_n(first, n);
                }

            public:
                block_stream_processor(StateAccumulator &s) : acc(s), cache(cache_type()), cache_seen(0) {
                }

                virtual ~block_stream_processor() {
                    if (cache_seen != 0) {
                        process_block(cache_seen * value_bits);
                        cache_seen = 0;
                    }
                }

                template<typename InputIterator>
                inline void operator()(InputIterator b, InputIterator e, std::random_access_iterator_tag) {
                    return update_n(b, e);
                }

                template<typename InputIterator, typename Category>
                inline void operator()(InputIterator first, InputIterator last, Category) {
#pragma clang loop unroll(full)
                    while (first != last) {
                        update_one(*first++);
                    }
                }

                template<typename ValueType>
                inline void operator()(const ValueType &value) {
                    return update_one(value);
                }

                template<typename InputIterator>
                inline void operator()(InputIterator b, InputIterator e) {
                    typedef typename std::iterator_traits<InputIterator>::iterator_category cat;
                    return operator()(b, e, cat());
                }

                template<typename ValueType>
                inline void operator()(const std::initializer_list<ValueType> &il) {
                    return operator()(il.begin(), il.end());
                }

                void reset() {
                    cache_seen = 0;
                }

                StateAccumulator &acc;

                length_type cache_seen;
                cache_type cache;
            };
        }    // namespace block
    }        // namespace crypto3
}    // namespace boost

#endif    // CRYPTO3_BLOCK_BLOCK_STATE_PREPROCESSOR_HPP


// Copyright 2019 ETH Zürich, Thomas Schöps
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// 1. Redistributions of source code must retain the above copyright notice,
//    this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright notice,
//    this list of conditions and the following disclaimer in the documentation
//    and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its contributors
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.

#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include <libvis/eigen.h>
#include <libvis/image_display.h>
#include <libvis/libvis.h>
#include <libvis/logging.h>

#include "badslam/direct_ba.h"
#include "badslam/kernels.h"
#include "badslam/util.cuh"
#include "badslam/render_window.h"
#include "badslam/cuda_depth_processing.cuh"

using namespace vis;

// This checks that the surfel depth optimization in bundle adjustment works
// correctly. It sets up a keyframe and a scene with surfels that match its
// depth image. The it perturbs the keyframe's depth map and tests whether
// optimizing the surfel positions moves them accordingly.
static void TestGeometryOptimizationWithGeometricResidual(bool use_pcg) {
  srand(0);

  // Initialize camera
  constexpr int width = 640;
  constexpr int height = 480;
  const float camera_parameters[4] = {0.5f * height, 0.5f * height, 0.5f * width - 0.5f, 0.5f * height - 0.5f};
  PinholeCamera4f camera(width, height, camera_parameters);

  // Set keyframe pose
  Matrix<float, 6, 1> global_tr_frame_log;
  global_tr_frame_log << 0.1f, 0.2f, 0.3f, 0.4f, 0.5f, 0.6f;
  SE3f global_tr_frame = SE3f::exp(global_tr_frame_log);

  // Initialize CUDA stream
  cudaStream_t stream;
  cudaStreamCreate(&stream);

  // Initialize DirectBA
  constexpr float raw_to_float_depth = 1.f / 5000;
  DirectBA direct_ba(
      /*max_surfel_count*/ 1000 * 1000,
      raw_to_float_depth,
      /*baseline_fx*/ 40,
      /*sparse_surfel_cell_size*/ 1.f,
      /*surfel_merge_dist_factor*/ 0.8f,
      /*min_observation_count_while_bootstrapping_1*/ 1,
      /*min_observation_count_while_bootstrapping_2*/ 1,
      /*min_observation_count*/ 1,
      /*color_camera_initial_estimate*/ camera,
      /*depth_camera_initial_estimate*/ camera,
      /*pyramid_level_for_color*/ 0,
      /*use_depth_residuals*/ true,
      /*use_descriptor_residuals*/ true,
      nullptr,
      /*global_T_anchor_frame*/ SE3f());

  // Create a depth image.
  Image<u16> depth_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      if (x == 0 || y == 0 || x == width - 1 || y == height - 1) {
        depth_image(x, y) = numeric_limits<u16>::max();
      } else {
        depth_image(x, y) = (1 + 0.01f * (rand() % 100)) / raw_to_float_depth + 0.5f;
      }
    }
  }

  // Create a homogeneous color image.
  Image<Vec3u8> color_image(width, height);
  memset(color_image.data(), 0, color_image.stride() * color_image.height());

  shared_ptr<Keyframe> new_keyframe(new Keyframe(
      stream,
      /*frame_index*/ 0,
      direct_ba.depth_params(),
      direct_ba.depth_camera(),
      depth_image,
      color_image,
      global_tr_frame));

  // Force normals to surfel<->camera direction such that the optimization does
  // not change the pixel correspondence of surfels.
  Image<u16> normals_image(width, height);
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      Vec3f normal = -1 * camera.UnprojectFromPixelCenterConv(Vec2f(x, y)).normalized();
      normals_image(x, y) = ImageSpaceNormalToU16(normal.x(), normal.y());
    }
  }
  CUDABuffer<u16>* kf_normals_buffer = const_cast<CUDABuffer<u16>*>(&new_keyframe->normals_buffer());
  kf_normals_buffer->UploadAsync(/*stream*/ 0, normals_image);

  direct_ba.AddKeyframe(new_keyframe);

  // Create surfels from the depth map.
  direct_ba.CreateSurfelsForKeyframe(stream, /*filter_new_surfels*/ false, new_keyframe);

  // Change the depth map and test whether the depth optimization changes the surfels accordingly.
  for (int y = 0; y < height; ++ y) {
    for (int x = 0; x < width; ++ x) {
      depth_image(x, y) += (0.0001f * (rand() % 50)) / raw_to_float_depth;
    }
  }
  CUDABuffer<u16>* kf_depth_buffer = const_cast<CUDABuffer<u16>*>(&new_keyframe->depth_buffer());
  kf_depth_buffer->UploadAsync(stream, depth_image);

  constexpr int kIterationCount = 10;
  for (int i = 0; i < kIterationCount; ++ i) {
    direct_ba.BundleAdjustment(
        stream,
        /*optimize_depth_intrinsics*/ false,
        /*optimize_color_intrinsics*/ false,
        /*do_surfel_updates*/ false,
        /*optimize_poses*/ false,
        /*optimize_geometry*/ true,
        /*min_iterations*/ 10,
        /*max_iterations*/ 10,
        use_pcg,
        /*active_keyframe_window_start*/ 0,
        /*active_keyframe_window_end*/ direct_ba.keyframes().size() - 1,
        /*increase_ba_iteration_count*/ true);
  }

  vector<float> surfel_x(direct_ba.surfel_count());
  vector<float> surfel_y(direct_ba.surfel_count());
  vector<float> surfel_z(direct_ba.surfel_count());

  direct_ba.surfels()->DownloadPartAsync(kSurfelX * direct_ba.surfels()->ToCUDA().pitch(), direct_ba.surfel_count() * sizeof(float), stream, surfel_x.data());
  direct_ba.surfels()->DownloadPartAsync(kSurfelY * direct_ba.surfels()->ToCUDA().pitch(), direct_ba.surfel_count() * sizeof(float), stream, surfel_y.data());
  direct_ba.surfels()->DownloadPartAsync(kSurfelZ * direct_ba.surfels()->ToCUDA().pitch(), direct_ba.surfel_count() * sizeof(float), stream, surfel_z.data());

  cudaStreamSynchronize(stream);

  // NOTE: There is an error pattern visible in this visualization. I think
  //       it comes from limited numerical accuracy.
  constexpr bool kShowErrorVisualization = false;
  Image<Vec3u8> error_visualization(width, height);
  if (kShowErrorVisualization) {
    for (int y = 0; y < height; ++ y) {
      for (int x = 0; x < width; ++ x) {
        error_visualization(x, y) = Vec3u8(0, 0, 0);
      }
    }
  }

  SE3f frame_tr_global = global_tr_frame.inverse();
  Mat3f frame_r_global = frame_tr_global.rotationMatrix();
  const Vec3f& frame_t_global = frame_tr_global.translation();

  int num_fails = 0;
  for (usize i = 0; i < direct_ba.surfel_count(); ++ i) {
    Vec3f cam_space_surfel = frame_r_global * Vec3f(surfel_x[i], surfel_y[i], surfel_z[i]) + frame_t_global;
    Vec2f pxy;
    if (direct_ba.depth_camera().ProjectToPixelCornerConvIfVisible(cam_space_surfel, 0, &pxy)) {
      float expected_z = raw_to_float_depth * depth_image(pxy.x(), pxy.y());
      float actual_z = cam_space_surfel.z();
      float error = fabs(actual_z - expected_z);

      constexpr float kEpsilon = 1e-4f;
      if (error > kEpsilon) {
        ++ num_fails;
        if (kShowErrorVisualization) {
          error_visualization(pxy.x(), pxy.y()) = Vec3u8(255, 0, 0);
        }
      } else {
        if (kShowErrorVisualization) {
          u8 intensity = std::min<float>(255, (255.99f / kEpsilon) * error);
          error_visualization(pxy.x(), pxy.y()) = Vec3u8::Constant(intensity);
        }
      }
    }
  }

  EXPECT_EQ(0, num_fails);

  if (kShowErrorVisualization) {
    ImageDisplay debug_display;
    debug_display.Update(error_visualization, "Surfel depth error visualization");
    std::getchar();
  }

  cudaStreamDestroy(stream);
}

TEST(Optimization, AlternatingGeometryOptimizationWithGeometricResidual) {
  TestGeometryOptimizationWithGeometricResidual(/*use_pcg*/ false);
}

TEST(Optimization, PCGGeometryOptimizationWithGeometricResidual) {
  TestGeometryOptimizationWithGeometricResidual(/*use_pcg*/ true);
}


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

#include "SimpleTraversabilityRadialLUT.hpp"

#include <boost/tuple/tuple.hpp>

using namespace maps;
using namespace tools;

void SimpleTraversabilityRadialLUT::precompute(double distance, double resolutionX, double resolutionY)
{
    const double radius2 = distance * distance;

    width  = 2 * ceil(distance / resolutionX) + 1;
    height = 2 * ceil(distance / resolutionY) + 1;

    inDistance.resize(boost::extents[height][width]);
    std::fill(inDistance.data(), inDistance.data() + inDistance.num_elements(), false);

    parents.resize(boost::extents[height][width]);
    std::fill(parents.data(), parents.data() + parents.num_elements(), std::make_pair(-1, -1));

    centerX = width  / 2;
    centerY = height / 2;

    parents[centerY][centerX] = std::make_pair(-1, -1);

    for (unsigned int y = 0; y < height; ++y)
    {
        for (unsigned int x = 0; x < width; ++x)
        {
            int dx = (x - centerX);
            int dy = (y - centerY);

            if (dx == 0 && dy == 0)
                continue;

            double d2 = dx * dx * resolutionX * resolutionX + dy * dy * resolutionY * resolutionY;
            inDistance[y][x] = (d2 < radius2);

            if (abs(dx) > abs(dy))
            {
                int parentx = x - dx / abs(dx);
                int parenty = y - rint(static_cast<double>(dy) / abs(dx));
                parents[y][x] = std::make_pair(parenty, parentx);
            }
            else
            {
                int parentx = x - rint(static_cast<double>(dx) / abs(dy));
                int parenty = y - dy / abs(dy);
                parents[y][x] = std::make_pair(parenty, parentx);
            }
        }
    }
}

void SimpleTraversabilityRadialLUT::markAllRadius(grid::TraversabilityGrid& traversabilityGrid, int centerX, int centerY, int targetedValue) const
{
    int baseX = centerX - this->centerX;
    int baseY = centerY - this->centerY;

    for (unsigned int y = 0; y < height; ++y)
    {
        int mapY = baseY + y;
        if (mapY < 0 || mapY >= static_cast<int>(traversabilityGrid.getNumCells()[1]))
            continue;

        for (unsigned int x = 0; x < width; ++x)
        {
            int mapX = baseX + x;
            if (mapX < 0 || mapX >= static_cast<int>(traversabilityGrid.getNumCells()[0]))
                continue;

            if (inDistance[y][x] && traversabilityGrid.at(grid::Index(mapX, mapY)).getTraversabilityClassId() == targetedValue)
            {
                markSingleRadius(traversabilityGrid, centerX, centerY, x, y, targetedValue, targetedValue);
            }
        }
    }
}

void SimpleTraversabilityRadialLUT::markSingleRadius(grid::TraversabilityGrid& traversabilityGrid, int centerX, int centerY, int targetX, int targetY, int expectedValue, int markValue) const
{
    boost::tie(targetY, targetX) = parents[targetY][targetX];

    while (targetX != -1 && targetY != -1)
    {
        int mapX = centerX + targetX - this->centerX;
        int mapY = centerY + targetY - this->centerY;

        if (mapX < 0 || mapX >= static_cast<int>(traversabilityGrid.getNumCells()[0]) || mapY < 0 || mapY >= static_cast<int>(traversabilityGrid.getNumCells()[1]))
        {
            boost::tie(targetY, targetX) = parents[targetY][targetX];
            continue;
        }

        if (traversabilityGrid.at(grid::Index(mapX, mapY)).getTraversabilityClassId() != expectedValue)
        {
            traversabilityGrid.setTraversabilityAndProbability(markValue, 1, mapX, mapY);
        }

        boost::tie(targetY, targetX) = parents[targetY][targetX];
    }
}


#include <stan/math/mix.hpp>
#include <test/unit/math/mix/meta/eigen_utils.hpp>
#include <Eigen/Sparse>
#include <gtest/gtest.h>

TEST(MathMetaPrim, is_dense_dynamic_hierarchy_tests) {
  using Eigen::Array;
  using Eigen::Matrix;
  using stan::is_dense_dynamic;
  using stan::math::test::all_eigen_dense;
  all_eigen_dense<false, true, true, true, true, is_dense_dynamic, Matrix, -1,
                  -1>();
  all_eigen_dense<false, false, false, false, false, is_dense_dynamic, Matrix,
                  1, -1>();
  all_eigen_dense<false, false, false, false, false, is_dense_dynamic, Matrix,
                  -1, 1>();
  all_eigen_dense<false, true, true, true, true, is_dense_dynamic, Array, -1,
                  -1>();
  all_eigen_dense<false, false, false, false, false, is_dense_dynamic, Array, 1,
                  -1>();
  all_eigen_dense<false, false, false, false, false, is_dense_dynamic, Array,
                  -1, 1>();
}

TEST(MathMetaPrim, is_dense_dynamic_sparse_tests) {
  using Eigen::Array;
  using Eigen::Matrix;
  using stan::is_dense_dynamic;
  using stan::math::test::all_eigen_sparse;
  all_eigen_sparse<false, false, false, false, is_dense_dynamic>();
}

TEST(MathMetaPrim, is_dense_dynamic_decomp_tests) {
  using Eigen::Array;
  using Eigen::Matrix;
  using stan::is_dense_dynamic;
  using stan::math::test::all_eigen_dense_decomp;
  all_eigen_dense_decomp<false, is_dense_dynamic>();
}

TEST(MathMetaPrim, is_dense_dynamic_expr_tests) {
  using Eigen::Array;
  using Eigen::Matrix;
  using stan::is_dense_dynamic;
  using stan::math::test::all_eigen_dense_exprs;
  all_eigen_dense_exprs<true, true, false, true, is_dense_dynamic, Matrix, -1,
                        -1>();
  all_eigen_dense_exprs<false, false, false, true, is_dense_dynamic, Matrix, 1,
                        -1>();
  all_eigen_dense_exprs<false, false, false, true, is_dense_dynamic, Matrix, -1,
                        1>();
  all_eigen_dense_exprs<true, true, false, true, is_dense_dynamic, Array, -1,
                        -1>();
  all_eigen_dense_exprs<false, false, false, true, is_dense_dynamic, Array, 1,
                        -1>();
  all_eigen_dense_exprs<false, false, false, true, is_dense_dynamic, Array, -1,
                        1>();
}

TEST(MathMetaPrim, is_dense_dynamic_var_test) {
  using stan::is_dense_dynamic;
  using stan::math::var;
  using stan::math::var_value;

  EXPECT_TRUE((is_dense_dynamic<var_value<Eigen::MatrixXd>>::value));
  EXPECT_FALSE((is_dense_dynamic<var_value<Eigen::VectorXd>>::value));
}


//__BEGIN_LICENSE__
//  Copyright (c) 2009-2012, United States Government as represented by the
//  Administrator of the National Aeronautics and Space Administration. All
//  rights reserved.
//
//  The NGT platform is licensed under the Apache License, Version 2.0 (the
//  "License"); you may not use this file except in compliance with the
//  License. You may obtain a copy of the License at
//  http://www.apache.org/licenses/LICENSE-2.0
//
//  Unless required by applicable law or agreed to in writing, software
//  distributed under the License is distributed on an "AS IS" BASIS,
//  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
//  See the License for the specific language governing permissions and
//  limitations under the License.
// __END_LICENSE__

#include <iostream>
#include <fstream>
#include <sys/stat.h>
#include <vw/Image.h>
#include <vw/Image/PixelMath.h>
#include <vw/Image/PixelMask.h>
#include <vw/Image/MaskViews.h>
#include <vw/FileIO.h>
#include <math.h>
#include <time.h>

//using namespace std;
//using namespace vw;
#include <boost/math/distributions/chi_squared.hpp>
#include <boost/math/distributions/normal.hpp> // for normal_distribution
#include <boost/math/distributions/gamma.hpp>

#include <vw/Core.h>
#include <vw/Image.h>
#include <vw/FileIO.h>
#include <vw/Cartography.h>
#include <vw/Math.h>
using namespace vw;
using namespace vw::math;
using namespace vw::cartography;
#include <photk/Reconstruct.h>
#include <photk/Index.h>

template <class ViewT>
std::vector<float> build_histogram(ImageViewBase<ViewT> const& view) {
        typedef typename ViewT::pixel_accessor pixel_accessor;
        std::vector<float> result(DYNAMIC_RANGE);

        int num_valid = 0;
        pixel_accessor row_acc = view.impl().origin();
        for( int32 row=0; row < view.impl().rows(); ++row ) {
                pixel_accessor col_acc = row_acc;
                for( int32 col=0; col < view.impl().cols(); ++col ) {
                        if ( is_valid(*col_acc) ) {
                                result[ (*col_acc)[0] ] += 1.0;
                                ++num_valid;
                        }
                        col_acc.next_col();
                }
                row_acc.next_row();
        }

        // Renormalize the histogram values
        for (size_t i=0; i< result.size(); ++i)
                result[i] /= num_valid;

        return result;
}