//
//  main.cpp
//  sthlm
//
//  Created by Jilin Zhang on 2/14/17.
//  Copyright © 2017 Jilin Zhang. All rights reserved.
//

#include <iostream>
#include <fstream>
#include <string>
#include "unordered_map"
#include <sstream>
#include <vector>
#include <stdlib.h>
#include <math.h>
#include <iomanip>
#include <algorithm>
#include <boost/program_options.hpp>
//#include "omp.h"
#include "kseq.h"
#include <zlib.h>

KSEQ_INIT(gzFile,gzread);

//
struct Unitscore{
    long count;
    double discore;
    double monoscore;
};

std::unordered_map<std::string, Unitscore > kstore;

//g++ -std=c++11 main.cpp -o rmap
//g++ -std=c++11 main.cpp -o rmap -lboost_program_options -lz
/*
 basemap[] works by storing a very small array that maps a base to
 its complement, by dereferencing the array with the ASCII char's
 decimal value as the index
 (int) 'A' = 65;
 (int) 'C' = 67;
 (int) 'G' = 71;
 (int) 'T' = 84;
 (int) 'a' = 97;
 (int) 'c' = 99;
 (int) 'g' = 103;
 (int) 't' = 116;
 (int) 'N' = 78;
 (int) 'U' = 85;
 (int) 'u' = 117;
 for example: basemap['A'] => basemap[65] => 'T' etc.
 */
static const char basemap[255] =
{
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*   0 -   9 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  10 -  19 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  20 -  29 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  30 -  39 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  40 -  49 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /*  50 -  59 */
    '\0', '\0', '\0', '\0', '\0',  'U', '\0',  'G', '\0', '\0', /*  60 -  69 */
    '\0',  'C', '\0', '\0', '\0', '\0', '\0', '\0',  'N', '\0', /*  70 -  79 */
    '\0', '\0', '\0', '\0',  'A',  'A', '\0', '\0', '\0', '\0', /*  80 -  89 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0',  'u', '\0',  'g', /*  90 -  99 */
    '\0', '\0', '\0',  'c', '\0', '\0', '\0', '\0', '\0', '\0', /* 100 - 109 */
    '\0', '\0', '\0', '\0', '\0', '\0',  'a',  'a', '\0', '\0', /* 110 - 119 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 120 - 129 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 130 - 139 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 140 - 149 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 150 - 159 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 160 - 169 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 170 - 179 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 180 - 189 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 190 - 199 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 200 - 209 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 210 - 219 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 220 - 229 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 230 - 239 */
    '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', '\0', /* 240 - 249 */
    '\0', '\0', '\0', '\0', '\0'                                /* 250 - 254 */
};

void fetch_matrix(std::unordered_map<std::string, std::vector<double> > *map, std::string str, int ind, double count);
void show_matrix(std::unordered_map<std::string, std::vector<double> > *x);
void kmerizing (std::string *id, std::string* seq,  int kmer, int gap, std::string* name, std::unordered_map<std::string, std::vector<double> > *map);
double monoscoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len=0, int gap_pos=0);
double discoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len=0, int gap_pos=0);
inline double singleStringScore(double x, std::string* s, long p, std::unordered_map<std::string, std::vector<double> > *m);
inline double pairStringScore(double x, std::string* s, long sp, std::unordered_map<std::string,std::vector<double> > *m);
void Matrixfile(const char*x, std::unordered_map<std::string, std::vector<double> > *y, double pseudo_count);
int listParser(const char* list_name, std::string* names, int* length, std::unordered_map<std::string, std::vector<double> >* map,double pcount);
std::string seq_revcomp(std::string str);
void GenomeMap(std::string *id, std::string *seq, long monoLen, std::string *mname, std::unordered_map<std::string, std::vector<double> >* map );
void hashOut();
void unCenterCorr();

template<class T>
std::ostream& operator<<(std::ostream& os, const std::vector<T>& v){
         copy(v.begin(), v.end(), std::ostream_iterator<T>(os, " "));
         return os;
}

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

    std::unordered_map<std::string, std::vector<double> > matrixmap[100];
    int motif_len[100];
    std::string names_motif[100];
    double pseudo_count=0.00001;
    int motif_counts=0;
    char list_name[100],fasta_name[100];
    int kmer_length, gap_length;
    bool kmerflag=false ,outflag=false,corrflag=false, revcomflag=false;

    boost::program_options::options_description desc("Allowed options:");
    desc.add_options()
    ("help", "produce help message")
    ("fasta", boost::program_options::value<std::string>(), "input sequence file")
    ("list", boost::program_options::value<std::string> (), "specify a list of motifs")
    ("motif", boost::program_options::value<std::string>(), "specify one motif file")
    ("kmer,k", boost::program_options::value<int>(&kmer_length)->default_value(8), "specify the kmer length")
    ("gap,g", boost::program_options::value<int>(&gap_length)->default_value(0), "specify the gap length")
    ("kmerflag,f", boost::program_options::bool_switch(&kmerflag), "flag to switch to the kmer counting-score procedure")
    ("revcomp,r",boost::program_options::bool_switch(&revcomflag),"reverse compliment sequence")
    ("output,o",boost::program_options::bool_switch(&outflag),"output the kmer count-score")
    ("corr,c",boost::program_options::bool_switch(&corrflag),"output the correlation score (un centered cosine)")
    ;
    boost::program_options::variables_map vm;
    boost::program_options::positional_options_description p;
    p.add("fasta",-1);
    boost::program_options::store(boost::program_options::command_line_parser(argc,argv).options(desc).positional(p).run(), vm);
    boost::program_options::notify(vm);

    if (vm.count("list"))
    {
        const char* temx=vm["list"].as<std::string >().c_str();
        motif_counts= listParser(temx,names_motif, motif_len, matrixmap, pseudo_count);
    }
    if (vm.count("motif")) {
        const char* temname =vm["motif"].as<std::string>().c_str();
        motif_counts=1;
        strcpy(list_name,temname);
        names_motif[0]=(std::string) list_name;
        Matrixfile(temname, matrixmap, pseudo_count);
    }
    if (vm.count("fasta"))
    {
        const char * fastaname=vm["fasta"].as<std::string>().c_str();
        motif_len[0]=matrixmap[0].at("A").size();
        strcpy(fasta_name, fastaname );
    }
    if (vm.count("help")) {
        std::cout << "Usage:" << argv[0] << " [options]\n";
        std::cout << desc;
        return 0;
    }

// Load fasta sequence one by one;
    gzFile fp;
    kseq_t *seq;
    int l;
    fp=gzopen(fasta_name, "r");
    seq=kseq_init(fp);
    while((l=kseq_read(seq)) >=0){
        std::string seqnametmp, seqtmp;
        seqnametmp= &seq->name.s[0];
        seqtmp= &seq->seq.s[0];
        if(revcomflag)
            seqtmp=seq_revcomp(seqtmp);

        for(int i=0;i < motif_counts ; i++){
            if(kmerflag)
                kmerizing(&seqnametmp, &seqtmp,kmer_length,gap_length,&names_motif[i], &matrixmap[i]);
            else
                GenomeMap(&seqnametmp, &seqtmp,motif_len[i],&names_motif[i], &matrixmap[i]);
        }
    }
    kseq_destroy(seq);
    gzclose(fp);
    if(kmerflag && outflag)
        hashOut();
    if(kmerflag && corrflag)
        unCenterCorr();

    return 0;
}

//output the kmer count-score table
void hashOut(){
    for(auto it=kstore.begin(); it !=kstore.end(); it++)
        std::cout<<it->first << "\t" << it->second.count<< "\t" << it->second.discore <<std::endl;
//        std::cout<<it->first << "\t" << it->second.count<< "\t" << it->second.discore << "\t" << it->second.monoscore <<std::endl;

}

// function to calculate the uncentred the correlation, cosine
void unCenterCorr(){
    double sumx,sumy, sumz, sumxz, sumxy;
    for(auto it=kstore.begin(); it !=kstore.end(); it++){
        sumx+=it->second.count * it->second.count;
        sumy+=it->second.discore * it->second.discore ;
        sumz+=it->second.monoscore * it->second.monoscore  ;
        sumxy+= it->second.count * it->second.discore;
        sumxz+= it->second.count * it->second.monoscore;
    }
//    std::cout << sumxy/sqrt(sumx *sumy)<< "\t" << sumxz/sqrt(sumx *sumz) << std::endl;
    std::cout << sumxy/sqrt(sumx *sumy)<< std::endl;
}

// function to score all the locus in the sequence
void GenomeMap(std::string *id, std::string *seq, long monoLen, std::string *mname, std::unordered_map<std::string, std::vector<double> >* map ){
    unsigned long slen =seq->size();

    std::transform(seq->begin(), seq->end(), seq->begin(),::toupper);
    std::replace(seq->begin(),seq->end(),'T','U');

    for(long i=0; i < slen - monoLen +1 ; i++){
        std::string subseq=seq->substr(i,monoLen);
        if(subseq.find("N") != std::string::npos)
            continue;
        double mscore=0,dscore=0,rc_mscore=0,rc_dscore=0;
        std::string rc_subseq=seq_revcomp(subseq);

        mscore=singleStringScore(mscore, &subseq, 0, map);
        dscore= pairStringScore(dscore, &subseq,0, map);
        rc_mscore=singleStringScore(rc_mscore, &rc_subseq, 0,map);
        rc_dscore=pairStringScore(rc_dscore,&rc_subseq,0,map);
        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<mscore << std::fixed <<":"  << std::setprecision(5) <<dscore <<std::fixed << "\t+\t" << *mname <<std::endl;
        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<rc_mscore <<std::fixed << ":"  << std::setprecision(5) <<rc_dscore << std::fixed <<"\t-\t" <<*mname <<std::endl;
    }
}

// read the motif list
int listParser(const char* list_name, std::string* names, int* length, std::unordered_map<std::string, std::vector<double> >* map,double pcount){
    std::fstream motif_list;
    char motif_file_name[100];
    motif_list.open(list_name);
    int motif_line=0;

    while( motif_list.getline( motif_file_name,100) ){
        Matrixfile(motif_file_name, &map[motif_line],  pcount);
//        std::cout << motif_file_name << std::endl;
        length[motif_line]=(int) (map[motif_line])["A"].size();
        names[motif_line]=(std::string) motif_file_name;
        ++motif_line;
    }
//    std::cout << motif_line << " motifs loaded \n"<<std::endl;
    return motif_line;
}

//process the matrix input
void Matrixfile(const char* x, std::unordered_map<std::string, std::vector<double> > *y, double pseudo_count){
    std::fstream file;
    file.open(x);

    while(!file.is_open()){
        std::cout << "Something wrong with the filename" << std::endl;
        std::exit(EXIT_FAILURE);
    }
    int line_count=0;
    std::string iss;
    while(getline(file, iss)){
        line_count++;
        if(line_count <=2){
            continue;
        }
        else if(line_count >=3  and line_count <=18){
            fetch_matrix(y, iss, 1, pseudo_count);
        }
        else if(line_count >=19 && line_count<=22){
            fetch_matrix(y, iss, 2, pseudo_count);
        }
    }
}

//show the matrix and output or the screen
void show_matrix(std::unordered_map<std::string, std::vector<double> > *x){
    for(auto it=(*x).begin(); it !=(*x).end(); it++){
        std::cout << it->first << "\t";
        std::vector<double> tem= it->second;
        for( int i=0; i< tem.size(); ++i)
            std::cout <<  "\t" <<  tem[i];
        std::cout << std::endl;
    }
}
std::string seq_revcomp(std::string str){
    std::string rc_seq;
    for(int i=str.size()-1; i>=0; --i){
        rc_seq += (char) basemap[(int)str[i]];
    }
    return rc_seq;
}

//store the matrix into map table
void fetch_matrix(std::unordered_map<std::string, std::vector<double> > *map, std::string str, int ind, double count){
    std::vector<std::string> matrix;
    std::stringstream ms(str);
    std::string units;
    while(ms >> units){
        matrix.push_back(units);
    }
    unsigned long matrix_length_raw=matrix.size();
    std::string base_name= matrix.back();
    long matrix_length_new;
    if(ind ==1){
        base_name=base_name.substr(0,2);
        matrix_length_new=matrix_length_raw-2;
    }
    else if(ind==2){
        base_name=base_name.substr(14,1);
        matrix_length_new=matrix_length_raw-1;
    }
    double new_matrix[matrix_length_new];
    std::vector<double> hallo (new_matrix,new_matrix+ matrix_length_new);
    for(int i=0; i< matrix_length_new; i++){
        std::string::size_type sz;
        hallo[i]= std::stod(matrix[i], &sz);
        if(ind ==1)
            hallo[i] = std::log2((hallo[i]+ count)/(0.0625 + count));
        else
            hallo[i] = std::log2((hallo[i]+ count)/(0.25+ count));
    }
    map->insert({base_name,hallo});
//    return map;
}

//function to generate the kmer count-score table
void kmerizing (std::string* id, std::string* seq,  int kmer, int gap, std::string* name, std::unordered_map<std::string, std::vector<double> > *map){
    std::transform(seq->begin(), seq->end(), seq->begin(),::toupper);
    std::replace(seq->begin(),seq->end(),'T','U');
    unsigned long seq_length=seq->size();
    int raw_kseq_len=kmer+gap;

    for(unsigned long i=0; i < seq_length-raw_kseq_len +1; i++){
        std::string subseq=seq->substr(i,raw_kseq_len);
        if(subseq.find("N") != std::string::npos)
            continue;

        if(gap >0){
            for (int ii=1; ii <kmer ;ii++){
                double mscore,dscore;
                mscore=monoscoring(subseq, map, gap,ii);
                dscore=discoring(subseq, map, gap,ii);
                std::string tem=subseq;
                tem.replace(ii,gap, gap, '-' );

                if(kstore.find(tem) != kstore.end()){
                    kstore[tem].count +=1;
                }
                else
                    kstore.insert({tem,{1,dscore,mscore}});
            }
        }
        else{
            double mscore,dscore,rc_mscore, rc_dscore;
            //         std::string rc_subseq=seq_revcomp(subseq);
            mscore=monoscoring(subseq, map, 0,0);
            dscore=discoring(subseq, map, 0,0);
            std::string tem=subseq;

            if(kstore.find(tem) != kstore.end()){
                kstore[tem].count +=1;
            }
            else
                kstore.insert({tem,{1,dscore,mscore}});
        }

//        double mscore,dscore,rc_mscore,rc_dscore;
//        std::string rc_subseq=seq_revcomp(subseq);
//        mscore=monoscoring(subseq, map, 0,0);
//        dscore=discoring(subseq, map, 0,0);
//
//        rc_mscore=monoscoring(rc_subseq, map, 0,0);
//        rc_dscore=discoring(rc_subseq, map, 0,0);
//        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<mscore << std::fixed <<":"  << std::setprecision(5) <<dscore <<std::fixed << "\t+\t" << *name <<std::endl;
//        std::cout << *id << "\t"<< i << "\t" << std::setprecision(5) <<rc_mscore <<std::fixed << ":"  << std::setprecision(5) <<rc_dscore << std::fixed <<"\t-\t" <<*name <<std::endl;
//
    }
}

// The scoring function using the linear model only, which has considered all the conditions
double monoscoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len, int gap_pos){
    long monoLen=(*map)["A"].size();
    if(gap_len >0)
        subseq.replace(gap_pos,gap_len, gap_len, '-' );

    long kseq_prime_len= subseq.size();
    long cycle=kseq_prime_len+ monoLen;
    std::string valid_seq;
    double maximum_score=-1000;
    for(int p=0; p < cycle -1 ; p++){
        long pos;
        double score=0;
        if(p <kseq_prime_len-1){
            if(monoLen >= kseq_prime_len){
                valid_seq=subseq.substr(kseq_prime_len-(p+1),p+1);
                pos=0;
            }
            else if(monoLen < kseq_prime_len){
                pos=0;
                if(p <monoLen -1)
                    valid_seq=subseq.substr(kseq_prime_len-(p+1));
                else
                    valid_seq=subseq.substr(kseq_prime_len-(p+1),monoLen);
            }
        }
        else if(p >=kseq_prime_len -1){
            if(monoLen >=kseq_prime_len){
                pos=p+1-kseq_prime_len;
                if(p <monoLen-1)
                    valid_seq=subseq;
                else
                    valid_seq=subseq.substr(0,kseq_prime_len+monoLen-p-1);
                }
            else{
                valid_seq=subseq.substr(0,kseq_prime_len-(p-monoLen) -1);
                pos=p+1-kseq_prime_len;
            }
        }
        score= singleStringScore(score, &valid_seq, pos, map);
        maximum_score=(score>maximum_score)?score:maximum_score;
    }
    return maximum_score ;
}
// The scoring function for the dependency matrix
double discoring(std::string subseq, std::unordered_map<std::string, std::vector<double> > *map, int gap_len, int gap_pos){
    if(gap_len >0)
        subseq.replace(gap_pos,gap_len,gap_len,'-');
    long monoLen=(*map)["A"].size();
    long kseq_prime_len=subseq.size();
    double maximum_score=-1000;
    if(monoLen %2 ==0){ //matrix length is even
        for(int p=0; p < monoLen + kseq_prime_len -1; p++){
            std::string subs;
            double score=0;
            long pos;
            if( p< monoLen/2){
                if( p < kseq_prime_len-1){
                    subs=subseq.substr(kseq_prime_len-1-p,p+1); //for kmers longer than the 1/2 matrix length
                    pos=0;
                }
                else if(p >=kseq_prime_len-1){  //for kmers shorter than the 1/2 marix length
                    subs=subseq;
                    pos=p-kseq_prime_len+1;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= monoLen/2   && (p - monoLen/2 < kseq_prime_len -1)){
                std::string left_seq; // leftover from the kmer which has been cut for dinucleotide matrix
                long single_pos;
                if(kseq_prime_len <= monoLen && kseq_prime_len >= 0.5 * monoLen){
                    if(2*(p-(monoLen/2 -1)) <=kseq_prime_len){
                        subs=subseq.substr(kseq_prime_len-2*(p-(monoLen/2 -1)),2*(p-(monoLen/2 -1)));
                        single_pos=p+1-subs.size();
                        if(p+1 >= kseq_prime_len){
                            pos=p+1-kseq_prime_len;
                            left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen/2-1))); //if($kseq_prime_len-2*($p-($mono_len/2 -1)) > 0);
                        }
                        else{
                            pos=0;
                            left_seq=subseq.substr(kseq_prime_len-(p+1),p+1-2*(p-(monoLen/2-1)));
                        }
                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2 )+1) );
                        pos=p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                        if(p >= monoLen  ){
                            std::string left_seq_pre=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2-1))));
                            left_seq=left_seq_pre.substr(0,kseq_prime_len-(p+1-monoLen)-subs.size());
                        }
                        else
                            left_seq=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2 -1))));
                    }
//                    std::cout << subseq<< "\t" << subs << "\t" << single_pos << "\t" << pos << std::endl;
                }
                else if(kseq_prime_len > monoLen){
                    std::string left_seq_pre;
                    if(p < monoLen-1){
                        subs=subseq.substr(kseq_prime_len-2*(p-(monoLen/2 -1)),2*(p-(monoLen/2 -1)));
                        if(2*(p-(monoLen/2 -1)) <kseq_prime_len)
                            left_seq_pre =subseq.substr(0,kseq_prime_len-2*(p-(monoLen/2 -1)));
                        left_seq=left_seq_pre.substr(kseq_prime_len - (p+1));
                        pos=0;
                        single_pos=p+1-subs.size();
                    }
                    else if( p >= monoLen-1 && p <= kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-(p+1),monoLen);
                        pos=0;
                        single_pos=0;

                    }
                    else if(p > kseq_prime_len -1){
                        subs=subseq.substr(0,2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        left_seq_pre=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        left_seq=left_seq_pre.substr(0, left_seq_pre.size()-(p+1-monoLen));
                        pos=p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                    }
//                    std::cout << subseq<< "\t" << subs << "\t" << single_pos << "\t" << pos << std::endl;
                }
                else if(kseq_prime_len <  0.5 * monoLen){
                    if(2*(p-(monoLen/2 -1)) <=kseq_prime_len){
                        subs=subseq.substr(kseq_prime_len-2*(p-(monoLen/2 -1)),2*(p-(monoLen/2 -1)));
                        pos=p+1-kseq_prime_len;
                        left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen/2 -1)));
                        single_pos=p+1-subs.size();

                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        pos=p-kseq_prime_len+1+subs.size();
                        left_seq=subseq.substr(2*(kseq_prime_len-(p-(monoLen/2 -1))));
                        single_pos=p+1-kseq_prime_len;
                    }
                }

                score=pairStringScore(score,&subs,single_pos,map);
//                std::cout << subseq<< "\t" << subs << "\t" << single_pos << "\t" << pos << std::endl;

                if(!left_seq.empty())
                    score= singleStringScore(score, &left_seq, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= monoLen/2 + kseq_prime_len -1){
                if(p <monoLen){
                    subs=subseq;
                    pos=p+1-kseq_prime_len;
                }
                else if(p >= monoLen){
                    subs=subseq.substr(0,kseq_prime_len-(p-monoLen)-1);
                    pos=p+1-kseq_prime_len;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
        }
    }
    else{
        for(int p=0; p < monoLen + kseq_prime_len -1; p++){
            std::string subs;
            double score=0;
            long pos;
            if( p < (monoLen-1)/2){
                if(p < kseq_prime_len-1){
                    subs=subseq.substr(kseq_prime_len-1-p,p+1); // for kmers longer than the 1/2 matrix length
                    pos=0;
                }
                else if(p >=kseq_prime_len-1){  //for kmers shorter than the 1/2marix length
                    subs=subseq;
                    pos=p-kseq_prime_len+1;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= (monoLen-1)/2  && (p - (monoLen-1)/2 <= kseq_prime_len -1)){
                std::string left_seq;
                long single_pos;
                if(kseq_prime_len <= monoLen && kseq_prime_len >= 0.5 * (monoLen-1)){
                    if(2*(p-(monoLen-1)/2 ) <=kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-2*(p+1-(monoLen-1)/2)+1,2*(p+1-(monoLen-1)/2)-1);
                        single_pos=p+1-subs.size();
                        pos=0;
                        if(p+1 >= kseq_prime_len){
                            pos=p+1-kseq_prime_len;
                            left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen-1)/2)-1); //if($kseq_prime_len-2*($    p-($mono_len/2 -1)) > 0);
                        }
                        else{
                            pos=0;
                            left_seq=subseq.substr(kseq_prime_len-(p+1),p+1-2*(p-(monoLen-1)/2)-1);
                        }
                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2 ))+1);
                        pos= p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                        if(p >= monoLen){
                            std::string left_seq_pre=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                            left_seq=left_seq_pre.substr(0,kseq_prime_len-(p+1-monoLen)-subs.size());
                        }
                        else{
                            left_seq=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                        }
                    }
                }
                else if( kseq_prime_len > monoLen){
                    std::string left_seq_pre;
                    if(p < monoLen-1){
                        subs=subseq.substr(kseq_prime_len-2*(p+1-(monoLen-1)/2)+1,2*(p+1-(monoLen-1)/2)+1);
                        if(2*(p-(monoLen/2 -1)) < kseq_prime_len)
                            left_seq_pre=subseq.substr(0,kseq_prime_len-2*(p+1-(monoLen-1)/2)+1);
                        left_seq=left_seq_pre.substr(kseq_prime_len - (p+1));
                        pos=0;
                        single_pos=p+1-subs.size();
                        }
                    else if( p >= monoLen-1 && p <= kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-(p+1),monoLen);
                        pos=0;
                        single_pos=0;
                        }
                    else if(p > kseq_prime_len -1){
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2 ))+1);
                        left_seq_pre=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2 ))+1);
                        left_seq=left_seq_pre.substr(0, left_seq_pre.size()-(p+1-monoLen));
                        pos=p-kseq_prime_len+1+subs.size();
                        single_pos=p+1-kseq_prime_len;
                    }
                }
                else if(kseq_prime_len <  0.5 * (monoLen-1)){
                    if(2*(p-(monoLen-1)/2 ) <=kseq_prime_len-1){
                        subs=subseq.substr(kseq_prime_len-2*(p+1-(monoLen-1)/2)+1,2*(p+1-(monoLen-1)/2)+1);
                        pos=p+1-kseq_prime_len;
                        left_seq=subseq.substr(0,kseq_prime_len-2*(p-(monoLen-1)/2)-1);
                        single_pos=p+1-subs.size();
                    }
                    else{
                        subs=subseq.substr(0,2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                        pos= p-kseq_prime_len+1+ subs.size();
                        left_seq=subseq.substr(2*(kseq_prime_len-(p+1-(monoLen-1)/2))+1);
                        single_pos=p+1-kseq_prime_len;
                    }
                }
                score=pairStringScore(score,&subs,single_pos,map);
                if(!left_seq.empty())
                    score= singleStringScore(score, &left_seq, pos, map);
                maximum_score=(score > maximum_score)?score:maximum_score;
            }
            else if(p >= (monoLen-1)/2 +kseq_prime_len -1){
                if(p <monoLen){
                    subs=subseq;
                    pos=p+1-kseq_prime_len;
                    }
                else if(p >= monoLen){
                    subs=subseq.substr(0,kseq_prime_len-(p-monoLen)-1);
                    pos=p+1-kseq_prime_len;
                }
                score= singleStringScore(score, &subs, pos, map);
                maximum_score=(score > maximum_score )?score:maximum_score;
            }
        }
    }
    return maximum_score;
}
//sub-routines for the dangling sequence
inline double singleStringScore(double x, std::string* s, long p, std::unordered_map<std::string, std::vector<double> > *m){
    for(int j=0; j<s->size(); j++){
        std::string letter=s->substr(j,1);
        if(letter == "-")
            continue;
        x+= (*m)[letter][p+j];
    }
    return x;
}
//sub-routines for the paired sequence
inline double pairStringScore(double x, std::string * s, long sp, std::unordered_map<std::string,std::vector<double> > *m){
    long subs_len=s->size();
    long diLen=(*m)["AA"].size();
    int half_subs_len;
    if(subs_len %2==0)
        half_subs_len=0.5* subs_len ;
    else
        half_subs_len=0.5* (subs_len-1) ;

    for (int j=0; j <= half_subs_len -1 ; j++){
        std::string letterA=s->substr(j,1);
        std::string letterB=s->substr(subs_len-j-1,1);
        std::string letter;

        if(letterA =="-" && letterB=="-")
            continue;
        else if(letterA =="-" ||letterB=="-"){
            long spos;
            if(letterB=="-"){
                spos=sp +j ;
                letter=letterA;
            }//#letterA is not gap so use the postion from the subs' beginning
            else if(letterA=="-"){
                spos=sp+(subs_len-1)-j; //#B is not gap, so from the end of subs
                letter=letterB;
            }
            x+=(*m)[letter][spos];
        }
        else{
            letter=letterA+letterB;
//            std::cout << *s << "\t" << letter  << "\t" << diLen-half_subs_len+j<< std::endl;
            x+=(*m)[letter][diLen-half_subs_len+j];
        }
    }
    return x;
}


/*

Copyright (c) 2005-2021, University of Oxford.
All rights reserved.

University of Oxford means the Chancellor, Masters and Scholars of the
University of Oxford, having an administrative office at Wellington
Square, Oxford OX1 2JD, UK.

This file is part of Chaste.

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.
 * Neither the name of the University of Oxford 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.

*/

#ifndef EXTENDEDBIDOMAINTISSUE_HPP_
#define EXTENDEDBIDOMAINTISSUE_HPP_

#include "ChasteSerialization.hpp"
#include <boost/serialization/base_object.hpp>

#include <vector>
#include "UblasMatrixInclude.hpp"

#include "AbstractStimulusFunction.hpp"
#include "AbstractStimulusFactory.hpp"
#include "AbstractConductivityTensors.hpp"

#include "AbstractCardiacTissue.hpp"

/**
 * Class that provides functionalities to specify a tissue within the context of the extended bidomain framework.
 *
 * The extended bidomain equations are of the form:
 *
 *  - div ( sigma_i1 grad Phi_1 ) + Am1*Cm1*d(phi_1)/dt - Am2*Cm2*d(phi_e)/dt + Am1*I_ion1 - Am1*I_stim1 + Amgap*G_gap*(phi_1 - phi_2)
 *  - div ( sigma_i2 grad Phi_2 ) + Am2*Cm2*d(phi_2)/dt - Am2*Cm2*d(phi_e)/dt + Am2*I_ion2 - Am2*I_stim2 - Amgap*G_gap*(phi_1 - phi_2)
 *   div ( sigma_e grad Phi_e ) + div ( sigma_i1 grad Phi_1 ) + div ( sigma_i2 grad Phi_2 )  = I_stim
 *
 *   The unknowns are:
 *
 *   - Phi_1 (intracellular potential of the first cell).
 *   - Phi_2 (intracellular potential of the second cell).
 *   - Phi_e (extracellular potential).
 *
 *   Am1, Am2 and Amgap are surface-to-volume ratios for first cell, second cell and gap junction. User can set their values.
 *   Cm1 and cm2 are capaciatnce values of first and second cell respectively
 *   sigma_i1 and sigma_i2 are intracellular conductivity tensors of first and second cell respectively
 *   sigma_e is the conductivity tensor for the extracellular space
 *   G_gap is the conductance (in ms/cm2) of the gap junction channel.
 *
 *
 *
 */
template <unsigned SPACE_DIM>
class ExtendedBidomainTissue : public virtual AbstractCardiacTissue<SPACE_DIM>
{
private:
    friend class TestExtendedBidomainTissue; // for testing.

    /** Needed for serialization. */
    friend class boost::serialization::access;
    /**
     * Archive the member variables.
     *
     * @param archive
     * @param version
     */
    template<class Archive>
    void serialize(Archive & archive, const unsigned int version)
    {
        archive & boost::serialization::base_object<AbstractCardiacTissue<SPACE_DIM> >(*this);
        // Conductivity tensors are dealt with by HeartConfig, and the caches get regenerated.

        archive & mAmFirstCell;
        archive & mAmSecondCell;
        archive & mAmGap;
        archive & mCmFirstCell;
        archive & mCmSecondCell;
        archive & mGGap;
        archive & mUserSuppliedExtracellularStimulus;
    }

    /** Intracellular conductivity tensors for the second cell.*/
    AbstractConductivityTensors<SPACE_DIM, SPACE_DIM> *mpIntracellularConductivityTensorsSecondCell;

    /**
     * Stores the values of the conductivities for the second cell. Accessible via get and set methods. The problem class will set it
     * This variable is a convenient interface for other classes. It is used to fill in mpIntracellularConductivityTensorsSecondCell.
     */
    c_vector<double, SPACE_DIM>  mIntracellularConductivitiesSecondCell;

    /** Extracellular conductivity tensors. */
    AbstractConductivityTensors<SPACE_DIM, SPACE_DIM> *mpExtracellularConductivityTensors;

    /**
     *  Cache containing all the stimulus currents for each node,
     *  replicated over all processes.
     */
    ReplicatableVector mExtracellularStimulusCacheReplicated;

    /**
     *  Cache containing all the stimulus currents for each node,
     *  replicated over all processes.
     */
    ReplicatableVector mGgapCacheReplicated;

    /**
     *  Cache containing all the ionic currents for each node for the seconed cell,
     *  replicated over all processes.
     */
    ReplicatableVector mIionicCacheReplicatedSecondCell;

    /**
     *  Cache containing all the stimulus currents for each node for the second cell,
     *  replicated over all processes.
     */
    ReplicatableVector mIntracellularStimulusCacheReplicatedSecondCell;

    /** The vector of cells (the second one). Distributed. */
    std::vector< AbstractCardiacCellInterface* > mCellsDistributedSecondCell;

    /** The vector of stimuli for the extracellular stimulus. Distributed. */
    std::vector<boost::shared_ptr<AbstractStimulusFunction> > mExtracellularStimuliDistributed;

    /** The vector of gap junction conductances. Distributed*/
    std::vector<double> mGgapDistributed;

    /**the Am for the first cell, set by the problem class and picked up by the assembler*/
    double mAmFirstCell;
    /**the Am for the second cell, set by the problem class and picked up by the assembler*/
    double mAmSecondCell;
    /**the Am for the gap junction, set by the problem class and picked up by the assembler*/
    double mAmGap;
    /**the Cm for the first cell, set by the problem class and picked up by the assembler*/
    double mCmFirstCell;
    /**the Cm for the second cell, set by the problem class and picked up by the assembler*/
    double mCmSecondCell;
    /**the conductance of the gap junction, in mS/cm2. Set by the problem class and picked up by the assembler*/
    double mGGap;

    /**
     * Whether the extracellular stimulus that is passed in was supplied by the user or not
     * (it could be the default zero implementation). Initialise to false (user did not pass in anything).
     */
    bool mUserSuppliedExtracellularStimulus;

    /**
     * Convenience method for extracellular conductivity tensors creation
     */
    void CreateExtracellularConductivityTensors();

    /**
     * The parent class AbstractCardiacTissue has a method UpdateCaches that updates some caches of general use.
     * This method updates more caches that are specific to extended bidomain problems, namely:
     *
     * - Iionic and intracellular stimulus for the second cell
     * - Extracellular stimulus
     * - Gap junction conductivities (Ggap)
     *
     * It is typically called right after the UpdateCaches method in the parent class.
     *
     * @param globalIndex  global index of the entry to update
     * @param localIndex  local index of the entry to update
     * @param nextTime  the next PDE time point, at which to evaluate the stimulus current
     */
    void UpdateAdditionalCaches(unsigned globalIndex, unsigned localIndex, double nextTime);

    /**
     * The parent class AbstractCardiacTissue has a method ReplicateCaches that replicates some caches of general use.
     * This method replicates more caches that are specific to extended bidomain problems, namely:
     *
     * - Iionic and intracellular stimulus for the second cell
     * - Extracellular stimulus
     * - Gap junction conductivities (Ggap)
     *
     * It is typically called right after the ReplicateCaches method in the parent class.
     */
    void ReplicateAdditionalCaches();

    /** vector of regions for Ggap heterogeneities*/
    std::vector<boost::shared_ptr<AbstractChasteRegion<SPACE_DIM> > > mGgapHeterogeneityRegions;
    /**values of heterogeneous Ggaps corresponding to mGgapHeterogeneityRegions. This has the same size as mGgapHeterogeneityRegions*/
    std::vector<double> mGgapValues;

public:

    /**
     * Constructor sets up extracellular conductivity tensors.
     * @param pCellFactory factory to pass on to the base class constructor
     * @param pCellFactorySecondCell factory to pass on to the base class constructor for the second cell
     * @param pExtracellularStimulusFactory factory for creating extracellular stimuli
     */
    ExtendedBidomainTissue(AbstractCardiacCellFactory<SPACE_DIM>* pCellFactory, AbstractCardiacCellFactory<SPACE_DIM>* pCellFactorySecondCell, AbstractStimulusFactory<SPACE_DIM>* pExtracellularStimulusFactory);

    /**
     *  Archiving constructor
     * @param rCellsDistributed  local cell models (recovered from archive)
     * @param rSecondCellsDistributed  local cell models for second cells (recovered from archive)
     * @param rExtraStimuliDistributed local extracellular stimuli (recovered from archive)
     * @param rGgapsDistributed distributed Ggaps (recovered from archive)
     * @param pMesh  a pointer to the AbstractTetrahedral mesh (recovered from archive).
     * @param intracellularConductivitiesSecondCell a vector with the orthotropic conductivities for the second cell (this is needed because the second cell values may not be taken from HeartConfig as the the ones for the first cell are).
     */
    ExtendedBidomainTissue(std::vector<AbstractCardiacCellInterface*> & rCellsDistributed,
                           std::vector<AbstractCardiacCellInterface*> & rSecondCellsDistributed,
                           std::vector<boost::shared_ptr<AbstractStimulusFunction> > & rExtraStimuliDistributed,
                           std::vector<double>& rGgapsDistributed,
                           AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* pMesh,
                           c_vector<double, SPACE_DIM>  intracellularConductivitiesSecondCell);

    /**
     * Destructor
     */
    virtual ~ExtendedBidomainTissue();

    /**
     * Sets the value of the conductivities for the second cell.
     *
     * @param conductivities the conductivities to be set.
     */
    void SetIntracellularConductivitiesSecondCell(c_vector<double, SPACE_DIM> conductivities);

    /**
     * @return a pointer to the second cell
     *
     * @param globalIndex the global index in the mesh
     */
    AbstractCardiacCellInterface* GetCardiacSecondCell( unsigned globalIndex );

    /**
     * @return a pointer to the extracellular stimulus. Useful for testing
     *
     * @param globalIndex the global index in the mesh
     */
    boost::shared_ptr<AbstractStimulusFunction> GetExtracellularStimulus( unsigned globalIndex );

    /**
     *  @return a reference to the vector of distributed cells (second cell). Needed for archiving.
     */
    const std::vector<AbstractCardiacCellInterface*>& rGetSecondCellsDistributed() const;

    /**
     *  @return a reference to the vector of distributed values of Ggaps. Needed for archiving.
     */
    const std::vector<double>& rGetGapsDistributed() const;

    /**
     *  @return a reference to the vector of distributed extracellular stimuli. Needed for archiving.
     */
    const std::vector<boost::shared_ptr<AbstractStimulusFunction> >& rGetExtracellularStimulusDistributed() const;

    /**
     * @return the intracellular conductivities of the second cell
     */
    c_vector<double, SPACE_DIM> GetIntracellularConductivitiesSecondCell() const;

    /**
     * Integrate the cell ODEs and update ionic current etc for each of the
     * cells, between the two times provided. This is a re-implementation from the version in the base class.
     *
     * @param existingSolution  the current voltage solution vector
     * @param time  the current simulation time
     * @param nextTime  when to simulate the cells until
     * @param updateVoltage (unused here)
     */
    virtual void SolveCellSystems(Vec existingSolution, double time, double nextTime, bool updateVoltage = false);

    /**
     * Convenience method for intracellular conductivity tensors creation for the second cell
     */
    void CreateIntracellularConductivityTensorSecondCell();

    /**
     *  Set the values of mCellHeterogeneityRegions and mGgapValues for the heterogeneities of Ggap.
     *
     *  @param rGgapHeterogeneityRegions a vector of (pointers to) heterogeneity regions for gap junctions
     *  @param rGgapValues a vector (of the same size as rGgapHeterogeneityRegions) with the respective values of Ggap for every region.
     */
    void SetGgapHeterogeneities ( std::vector<boost::shared_ptr<AbstractChasteRegion<SPACE_DIM> > > & rGgapHeterogeneityRegions, std::vector<double> rGgapValues);

    /**
     * Create the pattern of Ggap across the mesh based upon mCellHeterogeneityRegions, mGgapValues and mGgap. This will fill in mGgapDistributed.
     * It will set mGgap everywhere except in  the areas mCellHeterogeneityRegions[i] where it will put mGgapValues[i] instead.
     * If mCellHeterogeneityRegions (and mGgapValues) are empty, mGgap will be set everywhere.
     */
    void CreateGGapConductivities();

    /**
     * @return the extracellular conductivity tensor for the given element
     * @param elementIndex  index of the element of interest
     */
     const c_matrix<double, SPACE_DIM, SPACE_DIM>& rGetExtracellularConductivityTensor(unsigned elementIndex);

     /**
      * @return the intracellular conductivity tensor for the given element for tehs econd cell
      * @param elementIndex  index of the element of interest
      */
      const c_matrix<double, SPACE_DIM, SPACE_DIM>& rGetIntracellularConductivityTensorSecondCell(unsigned elementIndex);

     /** @return the entire ionic current cache for the second cell*/
     ReplicatableVector& rGetIionicCacheReplicatedSecondCell();

     /** @return the entire stimulus current cache for the second cell*/
     ReplicatableVector& rGetIntracellularStimulusCacheReplicatedSecondCell();

     /** @return the extracellular stimulus*/
     ReplicatableVector& rGetExtracellularStimulusCacheReplicated();

     /** @return the values of ggap*/
     ReplicatableVector& rGetGgapCacheReplicated();

     /**
      * @return Am for the first cell
      */
     double GetAmFirstCell();

     /**
      * @return Am for the second cell
      */
     double GetAmSecondCell();

     /**
      * @return Am for the gap junction
      */
     double GetAmGap();

     /**
      * @return Cm for the first cell
      */
     double GetCmFirstCell();

     /**
      * @return Cm for the second cell
      */
     double GetCmSecondCell();

     /**
      *  @return the conducatnce of the gap junction (mGGap)
      */
     double GetGGap();

     /**
      * @param value Am for the first cell
      */
     void SetAmFirstCell(double value);

     /**
      * @param value Am for the second cell
      */
     void SetAmSecondCell(double value);

     /**
      * @param value Am for the gap junction
      */
     void SetAmGap(double value);

     /**
      * @param value Cm for the first cell
      */
     void SetCmFirstCell(double value);

     /**
      * @param value Cm for the first cell
      */
     void SetCmSecondCell(double value);

     /**
      * @param value conductance, in mS of the gap junction
      */
     void SetGGap(double value);

     /**
      * This method gives access to the member variable mUserSuppliedExtracellularStimulus,
      * which is false by default but turned true if the user supplies an extracellular stimulus
      * in any form.
      *
      * @return true if the user supplied an extracellular stimulus.
      */
     bool HasTheUserSuppliedExtracellularStimulus();

     /**
      * This method allows modifications of the  mUserSuppliedExtracellularStimulus flag (false by default).
      * Other classes (e.g., Problem classes) can use this method to tell the Tissue that the user
      * specified an extracellular stimulus.
      *
      * @param flag ; true if you want to tell the Tissue object that the user supplied an extracellular stimulus explicitly
      */
     void SetUserSuppliedExtracellularStimulus(bool flag);

     /**
      * This method is the equivalent of SaveCardiacCells in the abstract class but save both cells of the extended bidomain tissue
      *
      * @param archive  the master archive; cells will actually be written to the process-specific archive.
      * @param version
      */
     template<class Archive>
     void SaveExtendedBidomainCells(Archive & archive, const unsigned int version) const
     {
         Archive& r_archive = *ProcessSpecificArchive<Archive>::Get();
         const std::vector<AbstractCardiacCellInterface*> & r_cells_distributed = this->rGetCellsDistributed();
         const std::vector<AbstractCardiacCellInterface*> & r_cells_distributed_second_cell = rGetSecondCellsDistributed();
         const std::vector<double> & r_ggaps_distributed = rGetGapsDistributed();

         r_archive & this->mpDistributedVectorFactory; // Needed when loading
         const unsigned num_cells = r_cells_distributed.size();
         r_archive & num_cells;
         for (unsigned i=0; i<num_cells; i++)
         {
             AbstractDynamicallyLoadableEntity* p_entity = dynamic_cast<AbstractDynamicallyLoadableEntity*>(r_cells_distributed[i]);
             bool is_dynamic = (p_entity != NULL);
             r_archive & is_dynamic;
             if (is_dynamic)
             {
 #ifdef CHASTE_CAN_CHECKPOINT_DLLS
                 ///\todo Dynamically loaded cell models aren't saved to archive in extended Bidomain
                 NEVER_REACHED;
                 //r_archive & p_entity->GetLoader()->GetLoadableModulePath();
 #else
                 // We should have thrown an exception before this point
                 NEVER_REACHED;
 #endif // CHASTE_CAN_CHECKPOINT_DLLS
             }
             r_archive & r_cells_distributed[i];
             r_archive & r_cells_distributed_second_cell[i];
             r_archive & r_ggaps_distributed[i];
         }
     }

     /**
      * Load our tissue from an archive. This is the equivalent of LoadCardiacCells in the abstract class.
      * it loads the two cells instead of only one.
      *
      * Handles the checkpoint migration case, deleting loaded cells immediately if they are
      * not local to this process.
      *
      * @param archive  the process-specific archive to load from
      * @param version  archive version
      * @param rCells  vector to fill in with pointers to local cells
      * @param rSecondCells vector to fill in with pointers to the second cells
      * @param rGgaps vector of values of gap junctions
      * @param pMesh  the mesh, so we can get at the node permutation, if any
      */
     template<class Archive>
     static void LoadExtendedBidomainCells(Archive & archive, const unsigned int version,
                                  std::vector<AbstractCardiacCellInterface*>& rCells,
                                  std::vector<AbstractCardiacCellInterface*>& rSecondCells,
                                  std::vector<double>& rGgaps,
                                  AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* pMesh)
     {
         assert(pMesh!=NULL);
         DistributedVectorFactory* p_factory;
         archive & p_factory;
         unsigned num_cells;
         archive & num_cells;
         rCells.resize(p_factory->GetLocalOwnership());
         rSecondCells.resize(p_factory->GetLocalOwnership());
         rGgaps.resize(p_factory->GetLocalOwnership());
 #ifndef NDEBUG
         // Paranoia
         assert(rCells.size() == rSecondCells.size());
         for (unsigned i=0; i<rCells.size(); i++)
         {
             assert(rCells[i] == NULL);
             assert(rSecondCells[i] == NULL);
         }
 #endif

         // We don't store a cell index in the archive, so need to work out what global
         // index this tissue starts up.  If we're migrating (so have an
         // original factory) we use the original low index; otherwise we use the current
         // low index.
         unsigned index_low = p_factory->GetOriginalFactory() ? p_factory->GetOriginalFactory()->GetLow() : p_factory->GetLow();

         for (unsigned local_index=0; local_index<num_cells; local_index++)
         {
             unsigned global_index = index_low + local_index;
             unsigned new_local_index = global_index - p_factory->GetLow();
             bool local = p_factory->IsGlobalIndexLocal(global_index);

             bool is_dynamic;
             archive & is_dynamic;

             if (is_dynamic)
             {
 #ifdef CHASTE_CAN_CHECKPOINT_DLLS
                 ///\todo Dynamically loaded cell models aren't loaded from archive in extended Bidomain
                 NEVER_REACHED;
                 // Ensure the shared object file for this cell model is loaded.
                 // We need to do this here, rather than in the class' serialization code,
                 // because that code won't be available until this is done...
//                 std::string shared_object_path;
//                 archive & shared_object_path;
//                 DynamicModelLoaderRegistry::Instance()->GetLoader(shared_object_path);
 #else
                 // Could only happen on Mac OS X, and will probably be trapped earlier.
                 NEVER_REACHED;
 #endif // CHASTE_CAN_CHECKPOINT_DLLS
             }

             AbstractCardiacCellInterface* p_cell;
             AbstractCardiacCellInterface* p_second_cell;
             double g_gap;
             archive & p_cell;
             archive & p_second_cell;
             archive & g_gap;
             if (local)
             {
                 rCells[new_local_index] = p_cell; // Add to local cells
                 rSecondCells[new_local_index] = p_second_cell;
                 rGgaps[new_local_index] = g_gap;
             }
             else
             {
                 //not sure how to cover this, we are already looping over local cells...
                 NEVER_REACHED;
                // Non-local real cell, so free the memory.
                // delete p_cell;
                // delete p_second_cell;
             }
         }
     }

     /**
      * This method is the equivalent of SaveCardiacCells but Saves the extracellular stimulus instead
      *
      * @param archive  the master archive; cells will actually be written to the process-specific archive.
      * @param version
      */
     template<class Archive>
     void SaveExtracellularStimulus(Archive & archive, const unsigned int version) const
     {
         Archive& r_archive = *ProcessSpecificArchive<Archive>::Get();
         const std::vector<boost::shared_ptr<AbstractStimulusFunction> > & r_stimulus_distributed = rGetExtracellularStimulusDistributed();
         r_archive & this->mpDistributedVectorFactory; // Needed when loading
         const unsigned num_cells = r_stimulus_distributed.size();
         r_archive & num_cells;
         for (unsigned i=0; i<num_cells; i++)
         {
             r_archive & r_stimulus_distributed[i];
         }
     }

     /**
      * This method is the equivalent of LoadCardiacCells but Load the extracellular stimulus instead
      *
      * @param archive  the master archive; cells will actually be written to the process-specific archive.
      * @param version
      * @param rStimuli the extracellular stimuli (will be filled from the archive).
      * @param pMesh the mesh (needed to work out number of nodes). Here it is assumed we have already unarchived the mesh somewhere and the pointer passed in is not NULL.
      */
     template<class Archive>
     void LoadExtracellularStimulus(Archive & archive, const unsigned int version,
                                               std::vector<boost::shared_ptr<AbstractStimulusFunction> >& rStimuli,
                                               AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* pMesh)
     {

        DistributedVectorFactory* p_factory;
        archive & p_factory;
        unsigned num_cells;
        archive & num_cells;
        rStimuli.resize(p_factory->GetLocalOwnership());
#ifndef NDEBUG
          // Paranoia
          for (unsigned i=0; i<rStimuli.size(); i++)
          {
              assert(rStimuli[i] == NULL);
          }
#endif

        // We don't store a cell index in the archive, so need to work out what global
        // index this tissue starts up.  If we're migrating (so have an
        // original factory) we use the original low index; otherwise we use the current
        // low index.
        unsigned index_low = p_factory->GetOriginalFactory() ? p_factory->GetOriginalFactory()->GetLow() : p_factory->GetLow();

        assert(pMesh!=NULL);
        //unsigned num_cells = pMesh->GetNumNodes();
        for (unsigned local_index=0; local_index<num_cells; local_index++)
        {
          unsigned global_index = index_low + local_index;

          unsigned new_local_index = global_index - p_factory->GetLow();
          bool local = p_factory->IsGlobalIndexLocal(global_index);

          boost::shared_ptr<AbstractStimulusFunction> p_stim;
          archive & p_stim;//get from archive

          if (local)
          {
              rStimuli[new_local_index] = p_stim; // Add stimulus to local cells
          }
          //otherwise we should delete, but I think shared pointers delete themselves?
        }
    }
};

 // Declare identifier for the serializer
 #include "SerializationExportWrapper.hpp"
 EXPORT_TEMPLATE_CLASS_SAME_DIMS(ExtendedBidomainTissue)

 namespace boost
 {
 namespace serialization
 {

 template<class Archive, unsigned SPACE_DIM>
 inline void save_construct_data(
     Archive & ar, const ExtendedBidomainTissue<SPACE_DIM> * t, const unsigned int file_version)
 {
     //archive the conductivity tensor of the second cell (which may not be dealt with by heartconfig)
     c_vector<double, SPACE_DIM>  intracellular_conductivities_second_cell = t->GetIntracellularConductivitiesSecondCell();
     //note that simple: ar & intracellular_conductivities_second_cell may not be liked by some boost versions
     for (unsigned i = 0; i < SPACE_DIM; i++)
     {
         ar & intracellular_conductivities_second_cell(i);
     }

     const AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* p_mesh = t->pGetMesh();
     ar & p_mesh;

     // Don't use the std::vector serialization for cardiac cells, so that we can load them
     // more cleverly when migrating checkpoints.
     t->SaveExtendedBidomainCells(ar, file_version);
     t->SaveExtracellularStimulus(ar, file_version);

     // Creation of conductivity tensors are called by constructor and uses HeartConfig. So make sure that it is
     // archived too (needs doing before construction so appears here instead of usual archive location).
     HeartConfig* p_config = HeartConfig::Instance();
     ar & *p_config;
     ar & p_config;
 }

 /**
  * Allow us to not need a default constructor, by specifying how Boost should
  * instantiate an instance (using existing constructor)
  */
 template<class Archive, unsigned SPACE_DIM>
 inline void load_construct_data(
     Archive & ar, ExtendedBidomainTissue<SPACE_DIM> * t, const unsigned int file_version)
 {
     //Load conductivities of the conductivity of the second cell.
     c_vector<double, SPACE_DIM>  intra_cond_second_cell;
     //note that simple: ar & intra_cond_second_cell may not be liked by some boost versions
     for (unsigned i = 0; i < SPACE_DIM; i++)
     {
         double cond;
         ar & cond;
         intra_cond_second_cell(i) = cond;
     }

     std::vector<AbstractCardiacCellInterface*> cells_distributed;
     std::vector<AbstractCardiacCellInterface*> cells_distributed_second_cell;
     std::vector<boost::shared_ptr<AbstractStimulusFunction> > extra_stim;
     std::vector<double> g_gaps;
     AbstractTetrahedralMesh<SPACE_DIM,SPACE_DIM>* p_mesh;
     ar & p_mesh;

     // Load only the cells we actually own
     t->LoadExtendedBidomainCells(
             *ProcessSpecificArchive<Archive>::Get(), file_version, cells_distributed, cells_distributed_second_cell, g_gaps, p_mesh);

     t->LoadExtracellularStimulus(
             *ProcessSpecificArchive<Archive>::Get(), file_version, extra_stim, p_mesh);

     // CreateIntracellularConductivityTensor() is called by AbstractCardiacTissue constructor and uses HeartConfig.
     // (as does CreateExtracellularConductivityTensor). So make sure that it is
     // archived too (needs doing before construction so appears here instead of usual archive location).
     HeartConfig* p_config = HeartConfig::Instance();
     ar & *p_config;
     ar & p_config;

     ::new(t)ExtendedBidomainTissue<SPACE_DIM>(cells_distributed, cells_distributed_second_cell, extra_stim, g_gaps, p_mesh, intra_cond_second_cell);
 }
 }
 } // namespace ...

#endif /*EXTENDEDBIDOMAINTISSUE_HPP_*/


//==================================================================================================
/*!
  @file

  @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_FUNCTION_FAST_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_FAST_HPP_INCLUDED

#include <boost/simd/config.hpp>
#include <boost/simd/detail/decorator.hpp>
#include <boost/simd/detail/dispatch.hpp>
#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
  @ingroup group-decorator

    calls a version of the functor
      that can do some agressive optimization at the cost of certain
      properties or corner cases of the original functor.

      These losses are of the `fast_math` kind.

    @par Semantic

    @code
    T r = fast_(func)(< func parameters >);
    @endcode

  **/
  template<typename T> auto fast_(T const& x) {}

} }
#endif

namespace boost { namespace simd
{
  struct fast_tag : decorator_<fast_tag>
  {
    using parent = decorator_<fast_tag>;
  };

  const detail::decorator<fast_tag> fast_ = {};
} }

#endif


#include <mutex>
#include <memory>
#include <iostream>
#include <boost/format.hpp>

#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/common/transforms.h>

#include <ros/ros.h>
#include <pcl_ros/point_cloud.h>
#include <tf_conversions/tf_eigen.h>
#include <tf/transform_broadcaster.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>

#include <nav_msgs/Odometry.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/MarkerArray.h>

#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>

#include <hdl_people_tracking/ClusterArray.h>

#include <hdl_people_detection/people_detector.h>
#include <hdl_people_detection/background_subtractor.hpp>

namespace hdl_people_detection {

/**
 * @brief A nodelet to detect people using a 3D LIDAR
 */
class HdlPeopleDetectionNodelet : public nodelet::Nodelet {
public:
  using PointT = pcl::PointXYZI;

  HdlPeopleDetectionNodelet() {}
  virtual ~HdlPeopleDetectionNodelet() {}

  void onInit() override {
    nh = getNodeHandle();
    mt_nh = getMTNodeHandle();
    private_nh = getPrivateNodeHandle();

    initialize_params();

    // publishers
    backsub_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("backsub_points", 5);
    cluster_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("cluster_points", 5);
    human_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("human_points", 5);
    detection_markers_pub = private_nh.advertise<visualization_msgs::MarkerArray>("detection_markers", 5);

    backsub_voxel_points_pub = private_nh.advertise<sensor_msgs::PointCloud2>("backsub_voxel_points", 1, true);
    backsub_voxel_markers_pub = private_nh.advertise<visualization_msgs::Marker>("backsub_voxel_marker", 1, true);

    clusters_pub = private_nh.advertise<hdl_people_tracking::ClusterArray>("clusters", 10);

    // subscribers
    globalmap_sub = nh.subscribe("/globalmap", 1, &HdlPeopleDetectionNodelet::globalmap_callback, this);
    if(private_nh.param<bool>("static_sensor", false)) {
      static_points_sub = mt_nh.subscribe("/velodyne_points", 32, &HdlPeopleDetectionNodelet::callback_static, this);
    } else {
      odom_sub.reset(new message_filters::Subscriber<nav_msgs::Odometry>(mt_nh, "/odom", 20));
      points_sub.reset(new message_filters::Subscriber<sensor_msgs::PointCloud2>(mt_nh, "/velodyne_points", 20));
      sync.reset(new message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>(*odom_sub, *points_sub, 20));
      sync->registerCallback(boost::bind(&HdlPeopleDetectionNodelet::callback, this, _1, _2));
    }
  }

private:
  /**
   * @brief initialize_params
   */
  void initialize_params() {
    double downsample_resolution = private_nh.param<double>("downsample_resolution", 0.1);
    boost::shared_ptr<pcl::VoxelGrid<PointT>> voxelgrid(new pcl::VoxelGrid<PointT>());
    voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution);
    downsample_filter = voxelgrid;

    NODELET_INFO("create people detector");
    detector.reset(new PeopleDetector(private_nh));
  }

  /**
   * @brief in case the sensor is fixed
   * @param points_msg
   */
  void callback_static(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    if(!globalmap) {
      NODELET_INFO("constructing globalmap from a points msg");
      globalmap_callback(points_msg);
      NODELET_INFO("done");
      return;
    }

    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    if(cloud->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }

    // downsampling
    pcl::PointCloud<PointT>::Ptr downsampled(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*downsampled);
    downsampled->header = cloud->header;
    cloud = downsampled;

    // background subtraction and people detection
    auto filtered = backsub->filter(cloud);
    auto clusters = detector->detect(filtered);

    publish_msgs(points_msg->header.stamp, filtered, clusters);
  }

  /**
   * @brief callback
   * @param odom_msg    sensor pose
   * @param points_msg  point cloud
   */
  void callback(const nav_msgs::OdometryConstPtr& odom_msg, const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    if(!globalmap) {
      NODELET_ERROR("globalmap has not been received!!");
      return;
    }

    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    if(cloud->empty()) {
      NODELET_ERROR("cloud is empty!!");
      return;
    }

    // downsampling
    pcl::PointCloud<PointT>::Ptr downsampled(new pcl::PointCloud<PointT>());
    downsample_filter->setInputCloud(cloud);
    downsample_filter->filter(*downsampled);
    downsampled->header = cloud->header;
    cloud = downsampled;

    // transform #cloud into the globalmap space
    const auto& position = odom_msg->pose.pose.position;
    const auto& orientation = odom_msg->pose.pose.orientation;
    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    transform.block<3, 1>(0, 3) = Eigen::Vector3f(position.x, position.y, position.z);
    transform.block<3, 3>(0, 0) = Eigen::Quaternionf(orientation.w, orientation.x, orientation.y, orientation.z).toRotationMatrix();
    pcl::transformPointCloud(*cloud, *cloud, transform);
    cloud->header.frame_id = globalmap->header.frame_id;

    // background subtraction and people detection
    auto filtered = backsub->filter(cloud);
    auto clusters = detector->detect(filtered);

    publish_msgs(points_msg->header.stamp, filtered, clusters);
  }

  void globalmap_callback(const sensor_msgs::PointCloud2ConstPtr& points_msg) {
    NODELET_INFO("globalmap received!");
    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>());
    pcl::fromROSMsg(*points_msg, *cloud);
    globalmap = cloud;

    NODELET_INFO("background subtractor constructed");
    double backsub_resolution = private_nh.param<double>("backsub_resolution", 0.2);
    int backsub_occupancy_thresh = private_nh.param<int>("backsub_occupancy_thresh", 2);

    backsub.reset(new BackgroundSubtractor());
    backsub->setVoxelSize(backsub_resolution, backsub_resolution, backsub_resolution);
    backsub->setOccupancyThresh(backsub_occupancy_thresh);
    backsub->setBackgroundCloud(globalmap);

    backsub_voxel_markers_pub.publish(backsub->create_voxel_marker());
    backsub_voxel_points_pub.publish(backsub->voxels());
  }

private:
  /**
   * @brief publish messages
   * @param stamp
   * @param filtered
   * @param clusters
   */
  void publish_msgs(const ros::Time& stamp, const pcl::PointCloud<pcl::PointXYZI>::Ptr& filtered, const std::vector<Cluster::Ptr>& clusters) const {
    if(clusters_pub.getNumSubscribers()) {
      hdl_people_tracking::ClusterArrayPtr clusters_msg(new hdl_people_tracking::ClusterArray());
      clusters_msg->header.frame_id = globalmap->header.frame_id;
      clusters_msg->header.stamp = stamp;

      clusters_msg->clusters.resize(clusters.size());
      for(int i=0; i<clusters.size(); i++) {
        auto& cluster_msg = clusters_msg->clusters[i];
        cluster_msg.is_human = clusters[i]->is_human;
        cluster_msg.min_pt.x = clusters[i]->min_pt.x();
        cluster_msg.min_pt.y = clusters[i]->min_pt.y();
        cluster_msg.min_pt.z = clusters[i]->min_pt.z();

        cluster_msg.max_pt.x = clusters[i]->max_pt.x();
        cluster_msg.max_pt.y = clusters[i]->max_pt.y();
        cluster_msg.max_pt.z = clusters[i]->max_pt.z();

        cluster_msg.size.x = clusters[i]->size.x();
        cluster_msg.size.y = clusters[i]->size.y();
        cluster_msg.size.z = clusters[i]->size.z();

        cluster_msg.centroid.x = clusters[i]->centroid.x();
        cluster_msg.centroid.y = clusters[i]->centroid.y();
        cluster_msg.centroid.z = clusters[i]->centroid.z();
      }

      clusters_pub.publish(clusters_msg);
    }

    if(backsub_points_pub.getNumSubscribers()) {
      backsub_points_pub.publish(filtered);
    }

    if(cluster_points_pub.getNumSubscribers()) {
      pcl::PointCloud<pcl::PointXYZI>::Ptr accum(new pcl::PointCloud<pcl::PointXYZI>());
      for(const auto& cluster : clusters) {
        std::copy(cluster->cloud->begin(), cluster->cloud->end(), std::back_inserter(accum->points));
      }
      accum->width = accum->size();
      accum->height = 1;
      accum->is_dense = false;

      accum->header.stamp = filtered->header.stamp;
      accum->header.frame_id = globalmap->header.frame_id;

      cluster_points_pub.publish(accum);
    }

    if(human_points_pub.getNumSubscribers()) {
      pcl::PointCloud<pcl::PointXYZI>::Ptr accum(new pcl::PointCloud<pcl::PointXYZI>());
      for(const auto& cluster : clusters) {
        if(cluster->is_human){
          std::copy(cluster->cloud->begin(), cluster->cloud->end(), std::back_inserter(accum->points));
        }
      }
      accum->width = accum->size();
      accum->height = 1;
      accum->is_dense = false;

      accum->header.stamp = filtered->header.stamp;
      accum->header.frame_id = globalmap->header.frame_id;

      human_points_pub.publish(accum);
    }

    if(detection_markers_pub.getNumSubscribers()) {
      detection_markers_pub.publish(create_markers(stamp, clusters));
    }
  }

  visualization_msgs::MarkerArrayConstPtr create_markers(const ros::Time& stamp, const std::vector<Cluster::Ptr>& clusters) const {
    visualization_msgs::MarkerArrayPtr markers(new visualization_msgs::MarkerArray());
    markers->markers.reserve(clusters.size());

    for(int i=0; i<clusters.size(); i++) {
      if(!clusters[i]->is_human) {
        continue;
      }

      visualization_msgs::Marker cluster_marker;
      cluster_marker.header.stamp = stamp;
      cluster_marker.header.frame_id = globalmap->header.frame_id;
      cluster_marker.action = visualization_msgs::Marker::ADD;
      cluster_marker.lifetime = ros::Duration(0.5);
      cluster_marker.ns = (boost::format("cluster%d") % i).str();
      cluster_marker.type = visualization_msgs::Marker::CUBE;

      cluster_marker.pose.position.x = clusters[i]->centroid.x();
      cluster_marker.pose.position.y = clusters[i]->centroid.y();
      cluster_marker.pose.position.z = clusters[i]->centroid.z();
      cluster_marker.pose.orientation.w = 1.0;

      cluster_marker.color.r = 0.0;
      cluster_marker.color.g = 0.0;
      cluster_marker.color.b = 1.0;
      cluster_marker.color.a = 0.4;

      cluster_marker.scale.x = clusters[i]->size.x();
      cluster_marker.scale.y = clusters[i]->size.y();
      cluster_marker.scale.z = clusters[i]->size.z();

      markers->markers.push_back(cluster_marker);
    }

    return markers;
  }

private:
  // ROS
  ros::NodeHandle nh;
  ros::NodeHandle mt_nh;
  ros::NodeHandle private_nh;

  // subscribers
  std::unique_ptr<message_filters::Subscriber<nav_msgs::Odometry>> odom_sub;
  std::unique_ptr<message_filters::Subscriber<sensor_msgs::PointCloud2>> points_sub;
  std::unique_ptr<message_filters::TimeSynchronizer<nav_msgs::Odometry, sensor_msgs::PointCloud2>> sync;

  ros::Subscriber globalmap_sub;
  ros::Subscriber static_points_sub;

  // publishers
  ros::Publisher backsub_points_pub;
  ros::Publisher backsub_voxel_points_pub;

  ros::Publisher cluster_points_pub;
  ros::Publisher human_points_pub;

  ros::Publisher detection_markers_pub;
  ros::Publisher backsub_voxel_markers_pub;

  ros::Publisher clusters_pub;

  // global map
  pcl::PointCloud<PointT>::Ptr globalmap;

  pcl::Filter<PointT>::Ptr downsample_filter;
  std::unique_ptr<BackgroundSubtractor> backsub;
  std::unique_ptr<PeopleDetector> detector;

};

}

PLUGINLIB_EXPORT_CLASS(hdl_people_detection::HdlPeopleDetectionNodelet, nodelet::Nodelet)


/****
   Copyright 2005-2007, Moshe Looks and Novamente LLC

   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
****/

#include "moses/moses.h"
#include "moses/optimization.h"
#include "moses/scoring_functions.h"
#include <boost/lexical_cast.hpp>
#include "reduct/reduct.h"
#include <iostream>

using namespace moses;
using namespace reduct;
using namespace boost;
using namespace std;

int main(int argc,char** argv) {
  vtree tr;
  while (cin.good()) {
    cin >> tr;
    if (!cin.good())
      break;

    logical_reduce(tr);

    representation rep(logical_reduction(),tr,infer_tree_type(tr));

    vtree tmp(rep.exemplar());

    for (int i=0;i<10;++i) {
      cout << rep.exemplar() << endl;

      eda::instance inst(rep.fields().packed_width());
      for (eda::field_set::disc_iterator it=rep.fields().begin_raw(inst);
	   it!=rep.fields().end_raw(inst);++it)
	it.randomize();

      cout << rep.fields().stream(inst) << endl;
      rep.transform(inst);
      cout << rep.exemplar() << endl;

      rep.clear_exemplar();
      assert(tmp==rep.exemplar());
    }
  }
}


#include <bits/stdc++.h>
#include <zlib.h>
#include <boost/algorithm/string.hpp>

using namespace std;

uint64_t l3_access_cycle_count = 38;
uint64_t total_dyn_ins_count = 163607974;
double dyn_ins_count_inc_ratio=0.013;

#define BUFFER_SIZE 2048
#define FANIN 100
#define FANIN_THRESHOLD 10

uint64_t find_prefetch_index(vector<pair<string,uint64_t>> &current_lb_record)
{
    uint64_t running_cycle_count = 0;
    for(uint64_t i = 0; i < current_lb_record.size(); i++)
    {
        running_cycle_count+=current_lb_record[i].second;
        if(running_cycle_count>=l3_access_cycle_count)return i;
    }
    return current_lb_record.size()-1;
}

bool find_prefetch_index_for_multiple_samples_with_fanin(string missed_pc, vector<vector<pair<string,uint64_t>>> &current_lb_record_list, vector<string> &result)
{
    set<string> candidates_array[FANIN];
    uint64_t candidates_array_count[FANIN];
    for(uint64_t i=0;i<FANIN;i++)candidates_array_count[i]=0;
    uint64_t current_index = 0;
    for(int i = 0; i< current_lb_record_list.size(); i++)
    {
        uint64_t start_index = find_prefetch_index(current_lb_record_list[i]);
        if(start_index==current_lb_record_list[i].size())return false;
        start_index+=1;
        bool not_found = true;
        int j = 0;
        for(j=0; j< current_index; j++)
        {
            set<string> &candidates = candidates_array[j];

            set<string> tmp;
            for(int k = start_index; k<current_lb_record_list[i].size();k++)
            {
                string pc = current_lb_record_list[i][k].first;
                if(pc!=missed_pc)
                {
                    if(candidates.find(pc)!=candidates.end())
                    {
                        tmp.insert(pc);
                    }
                }
            }
            if(tmp.size()!=0)
            {
                not_found = false;
                candidates.clear();
                for(auto it: tmp)
                {
                    candidates.insert(it);
                }
                candidates_array_count[j]++;
                break;
            }
        }
        if(not_found)
        {
            current_index+=1;
            if(current_index>=FANIN)return false;
            for(int k = start_index; k< current_lb_record_list[i].size();k++)
            {
                string pc = current_lb_record_list[i][k].first;
                if(pc!=missed_pc)
                {
                    candidates_array[current_index].insert(pc);
                }
            }
            if(candidates_array[current_index].size()<1)return false;
        }
        else
        {
            //j contains the first found set
        }
    }

    vector<pair<uint64_t,string>> sorted_result;
    for(int j=0;j< current_index; j++)
    {
        set<string> &candidates = candidates_array[j];
        if(candidates.size()<1)continue;
        vector<pair<int,string>> sorted_candidates;
        for(auto it: candidates)
        {
            int total_distance = 0;
            for(int i = 0; i< current_lb_record_list.size();i++)
            {
                for(int k=0;k<current_lb_record_list[i].size();k++)
                {
                    if(current_lb_record_list[i][k].first == it)
                    {
                        total_distance+=k;
                        break;
                    }
                }
            }
            sorted_candidates.push_back(make_pair(total_distance,it));
        }
        sort(sorted_candidates.begin(), sorted_candidates.end());
        //for(int i = 0; i< sorted_candidates.size(); i++)cout<<sorted_candidates[i].second<<";"<<sorted_candidates[i].first<<",";
        //cout<<sorted_candidates[0].second<<endl;
        sorted_result.push_back(make_pair(candidates_array_count[j],sorted_candidates[0].second));
    }
    sort(sorted_result.begin(),sorted_result.end());
    reverse(sorted_result.begin(),sorted_result.end());
    result.clear();
    for(int j=0;j<sorted_result.size();j++)
    {
        result.push_back(sorted_result[j].second);
        if(j>=FANIN_THRESHOLD-1)break;
    }
    return true;
}

bool find_prefetch_index_for_multiple_samples(string missed_pc, vector<vector<pair<string,uint64_t>>> &current_lb_record_list, string &result)
{
    result="";
    set<string> candidates;
    uint64_t start_index = find_prefetch_index(current_lb_record_list[0]);
    start_index+=1;
    if(start_index==current_lb_record_list[0].size())
    {
        //cout<<"Not found\n";
        return false;
    }
    for(int i = start_index; i<current_lb_record_list[0].size();i++)
    {
        string pc = current_lb_record_list[0][i].first;
        if(pc!=missed_pc)
        {
            candidates.insert(pc);
        }
    }
    bool found = true;
    for(int i = 1; i< current_lb_record_list.size(); i++)
    {
        set<string> tmp;
        start_index = find_prefetch_index(current_lb_record_list[i]);
        start_index+=1;
        if(start_index==current_lb_record_list[i].size())
        {
            found = false;
            break;
        }
        for(int j = start_index; j<current_lb_record_list[i].size();j++)
        {
            string pc = current_lb_record_list[i][j].first;
            if(pc!=missed_pc)
            {
                if(candidates.find(pc)!=candidates.end())
                {
                    tmp.insert(pc);
                }
            }
        }
        if(tmp.size()==0)
        {
            found = false;
            break;
        }
        candidates.clear();
        for(auto it: tmp)
        {
            candidates.insert(it);
        }
    }
    if(found)
    {
        vector<pair<int,string>> sorted_candidates;
        for(auto it: candidates)
        {
            int total_distance = 0;
            for(int i = 0; i< current_lb_record_list.size();i++)
            {
                for(int j=0;j<current_lb_record_list[i].size();j++)
                {
                    if(current_lb_record_list[i][j].first == it)
                    {
                        total_distance+=j;
                        break;
                    }
                }
            }
            sorted_candidates.push_back(make_pair(total_distance,it));
        }
        sort(sorted_candidates.begin(), sorted_candidates.end());
        //for(int i = 0; i< sorted_candidates.size(); i++)cout<<sorted_candidates[i].second<<";"<<sorted_candidates[i].first<<",";
        //cout<<sorted_candidates[0].second<<endl;
        result = sorted_candidates[0].second;
        return true;
    }
    else
    {
        //cout<<"Not found\n";
        return false;
    }
}

uint64_t string_to_u64(string a)
{
    istringstream iss(a);
    uint64_t value;
    iss>>value;
    return value;
}

void check_pc_miss_same_cache_line(string cache_line_address, string pc)
{
    assert((string_to_u64(pc)>>6)==string_to_u64(cache_line_address));
}

int main(int argc, char *argv[])
{
    if(argc<2)
    {
        cerr << "Usage ./exec data_file_path\n";
        return -1;
    }
    gzFile data_file = gzopen(argv[1], "rb");
    if(!data_file)
    {
        cerr << "Invalid data_file_path\n";
        return -1;
    }
    char buffer[BUFFER_SIZE];
    memset(buffer, 0, BUFFER_SIZE);
    if(gzbuffer(data_file, BUFFER_SIZE*128) == -1)
    {
        cerr << "GZ Input file buffer set unsuccessfull\n";
        return -1;
    }
    string line;
    vector<string> parsed;
    string cache_line_address,pc;
    unordered_map<string,vector<vector<pair<string,uint64_t>>>> profile;
    unordered_map<string,uint64_t> miss_counts;
    while( gzgets(data_file,buffer,BUFFER_SIZE) != Z_NULL )
    {
        line=buffer;
        boost::trim_if(line,boost::is_any_of(",\n"));
        boost::split(parsed,line,boost::is_any_of(",\n"),boost::token_compress_on);
        //for(int i =0;i<parsed.size();i++)cerr<<parsed[i]<<",";
        //cerr<<endl;
        if(parsed.size()<3)
        {
            cerr << "LBR data contains less than 3 branch records\n";
            return -1;
        }
        cache_line_address = parsed[0];
        pc = parsed[1];
        check_pc_miss_same_cache_line(cache_line_address, pc);
        vector<string> current_record;
        vector<pair<string,uint64_t>> current_lb_record;
        for(int i=2;i<parsed.size();i++)
        {
            //cerr<<parsed[i]<<endl;
            boost::split(current_record,parsed[i],boost::is_any_of(";"),boost::token_compress_on);
            if(current_record.size() != 2)
            {
                cerr << "LB-record does not contain exactly 2 entries\n";
                return -1;
            }
            current_lb_record.push_back(make_pair(current_record[0],stoi(current_record[1])));
        }
        if(profile.find(pc)==profile.end())
        {
            profile[pc]=vector<vector<pair<string,uint64_t>>>();
            miss_counts[pc]=0;
        }
        profile[pc].push_back(current_lb_record);
        miss_counts[pc]+=1;
    }
    vector<pair<uint64_t,string>> sorted_miss_pcs;
    for(auto it: miss_counts)
    {
        sorted_miss_pcs.push_back(make_pair(it.second, it.first));
    }
    sort(sorted_miss_pcs.begin(), sorted_miss_pcs.end());
    reverse(sorted_miss_pcs.begin(), sorted_miss_pcs.end());
    uint64_t permissible_prefetch_count = dyn_ins_count_inc_ratio * total_dyn_ins_count;
    uint64_t current_running_count = 0;

    unordered_map<string,set<string>> bbl_address_prefetch_map;

    for(int i=0;i<sorted_miss_pcs.size();i++)
    {
        //cout<<sorted_miss_pcs[i].second<<","<<sorted_miss_pcs[i].first<<",";
        string pc = sorted_miss_pcs[i].second;
        if(profile[pc].size()>1)
        {
            /*string result;
            bool is_found = find_prefetch_index_for_multiple_samples(pc, profile[pc],result);
            if(is_found)cout<<result;
            else
            {*/
            vector<string> results;
            bool is_found = find_prefetch_index_for_multiple_samples_with_fanin(pc, profile[pc],results);
            if(is_found)
            {
                //for(int j=0;j<results.size();j++)cout<<results[j]<<",";
                for(int j = 0;j<results.size();j++)
                {
                    string bbl_address = results[j];
                    if(bbl_address_prefetch_map.find(bbl_address)==bbl_address_prefetch_map.end())
                    {
                        bbl_address_prefetch_map[bbl_address]=set<string>();
                    }
                    bbl_address_prefetch_map[bbl_address].insert(pc);
                }
            }
            //}
        }
        else if(profile[pc].size()==1)
        {
            uint64_t index = find_prefetch_index(profile[pc][0]);
            index+=1;
            if(index==profile[pc][0].size())
            {
                //cerr<<"No good prefetch position found\n";
            }
            else
            {
                //cout<<profile[pc][0][index].first<<"->"<<profile[pc][0][index-1].first<<"\n";
                //cout<<profile[pc][0][index].first;

                string bbl_address = profile[pc][0][index].first;
                if(bbl_address_prefetch_map.find(bbl_address)==bbl_address_prefetch_map.end())
                {
                    bbl_address_prefetch_map[bbl_address]=set<string>();
                }
                bbl_address_prefetch_map[bbl_address].insert(pc);
            }
        }
        else
        {
            cerr<<"Miss without lbr profile\n";
            return -1;
        }
        //cout<<endl;

        current_running_count+=sorted_miss_pcs[i].first;
        if(current_running_count>=permissible_prefetch_count)break;
    }

    for(auto it:bbl_address_prefetch_map)
    {
        cout<<it.first<<" "<<it.second.size();
        for(auto set_it: it.second)
        {
            cout<<" "<<set_it;
        }
        cout<<endl;
    }
}


#include <string.h>
#include <iostream>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/json_parser.hpp>

using namespace std;
using namespace boost::property_tree;

#include <map>
#include <string>
#include <iostream>
#include <string.h>

#include <fc/crypto/elliptic.hpp>
#include <fc/crypto/ripemd160.hpp>
#include <fc/crypto/base58.hpp>
#include <fc/crypto/sha512.hpp>
#include <fc/io/json.hpp>
#include <fc/reflect/reflect.hpp>
#include <fc/io/raw.hpp>
#include <fc/variant_object.hpp>
#include <fc/exception/exception.hpp>
#include <graphene/chain/pts_address.hpp>
#include <graphene/chain/protocol/address.hpp>

#include "wallet_lib.hpp"

using namespace std;

struct binary_key
{
  binary_key(){}
  uint32_t check = 0;
  fc::ecc::public_key_data data;
};

FC_REFLECT( binary_key, (data)(check) )

std::string key_to_wif(const fc::sha256& secret )
{
  const size_t size_of_data_to_hash = sizeof(secret) + 1;
  const size_t size_of_hash_bytes = 4;
  char data[size_of_data_to_hash + size_of_hash_bytes];
  data[0] = (char)0x80;
  memcpy(&data[1], (char*)&secret, sizeof(secret));
  fc::sha256 digest = fc::sha256::hash(data, size_of_data_to_hash);
  digest = fc::sha256::hash(digest);
  memcpy(data + size_of_data_to_hash, (char*)&digest, size_of_hash_bytes);
  return fc::to_base58(data, sizeof(data));
}

/*
 * global parameters to hold the keys
 */

//cybex_priv_key_type active_priv_key, owner_priv_key, memo_priv_key;

static map<string, fc::ecc::private_key> stored_keys;
static string default_public_key = "";

void set_default_public_key(string pub_key_base58_str)
{
  default_public_key = pub_key_base58_str;
}

void clear_user_key()
{
    stored_keys.clear();
}

void add_user_key(string public_key, fc::ecc::private_key key)
{
    stored_keys.insert(map<string, fc::ecc::private_key>::value_type(public_key, key));
}

fc::ecc::private_key& get_private_key(string public_key)
{
    map<string, fc::ecc::private_key>::iterator iter;

    if(public_key == "")
        iter = stored_keys.find(default_public_key);
    else
        iter = stored_keys.find(public_key);
    if(iter != stored_keys.end())
        return iter->second;

    FC_THROW_EXCEPTION(fc::exception, "private key not found");
}

fc::mutable_variant_object get_dev_key(string type, string secret)
{
  fc::ecc::private_key priv_key = fc::ecc::private_key::regenerate(fc::sha256::hash(string(secret)));
  string private_key = key_to_wif( priv_key );
  fc::ecc::public_key pub_key = priv_key.get_public_key();
  struct binary_key bkey;
  bkey.data = pub_key.serialize();
  bkey.check = fc::ripemd160::hash( bkey.data.data, bkey.data.size() )._hash[0];

  auto data = fc::raw::pack( bkey );
  string public_key = "CYB" + fc::to_base58( data.data(), data.size() );

  auto dat = pub_key.serialize();
  fc::ripemd160 addr = fc::ripemd160::hash( fc::sha512::hash( dat.data, sizeof( dat ) ) );
  fc::array<char,24> bin_addr;

  memcpy( (char*)&bin_addr, (char*)&addr, sizeof( addr ) );
  auto checksum = fc::ripemd160::hash( (char*)&addr, sizeof( addr ) );
  memcpy( ((char*)&bin_addr)+20, (char*)&checksum._hash[0], 4 );
  string address = "CYB" + fc::to_base58( bin_addr.data, sizeof( bin_addr ) );

  graphene::chain::pts_address compress_pts_addr(pub_key, true, 56);
  graphene::chain::pts_address uncompress_pts_addr(pub_key, false, 56);

  fc::mutable_variant_object mvo;
  mvo( "private_key", private_key)
  ( "public_key", public_key)
  ( "address", address)
  ( "compressed", string(graphene::chain::address(compress_pts_addr)))
  ( "uncompressed", string(graphene::chain::address(uncompress_pts_addr)))
  ;

  add_user_key(public_key, priv_key);
  if(type == "active")
    default_public_key = public_key;

  return mvo;
  //return fc::json::to_string(mvo);
}

string get_user_key(string user_name, string password)
{
  clear_user_key();
  fc::mutable_variant_object mvo;
  mvo("active-key", get_dev_key("active", user_name + "active" + password))
     ("owner-key", get_dev_key("owner", user_name + "owner" + password))
     ("memo-key", get_dev_key("memo", user_name + "memo" + password))
  ;
  return fc::json::to_string(mvo);
}