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

#include <algorithm>
#include <vector>
#include <map>
#include <unordered_map>
#include <boost/property_tree/ptree.hpp>
#include <boost/random/uniform_int_distribution.hpp>

#include "simulation_config.h"

#include "simulate_engine_base.hpp"

#include "clotho/genetics/sequence_mutator.hpp"
#include "clotho/genetics/sequence_generator.hpp"

#include "clotho/genetics/individual_initializer.hpp"
#include "clotho/genetics/individual_selector.hpp"
#include "clotho/genetics/individual_reproduction.hpp"
#include "clotho/genetics/individual_generator.hpp"
#include "clotho/genetics/individual_fitness.hpp"
#include "clotho/genetics/individual_resetter.hpp"
//#include "clotho/genetics/individual_phenotyper.hpp"
//
#include "clotho/genetics/assortative_selector.hpp"

#include "clotho/genetics/population_phenotyper.hpp"

#include "infinite_site.hpp"

#include "clotho/genetics/recombiner.hpp"

#include "clotho/powerset/powerset_no_dup_pred.hpp"
#include "clotho/powerset/variable_subset_recombination.hpp"
#include "clotho/powerset/variable_subset_fitness.hpp"

#include "clotho/utility/parameter_space.hpp"

#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_mean.hpp>
#include <boost/accumulators/statistics/weighted_median.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>

#include "clotho/fitness/fitness_toolkit.hpp"

#include "clotho/genetics/pairwise_statistic.hpp"
#include "clotho/genetics/population_growth_toolkit.hpp"
#include "clotho/genetics/sequence_helper.hpp"

#include "qtl_logging_parameter.hpp"
#include "../population_parameter.hpp"
#include "clotho/random/seed_parameter.hpp"

namespace accum=boost::accumulators;

#include "clotho/utility/popcount.hpp"

#include "EffectSizeMatrix.hpp"

template < class URNG, class AlleleType, class LogType, class TimerType >
class simulate_engine : public qtl_logging_parameter {
public:
    typedef simulate_engine_base< URNG, AlleleType, LogType, TimerType > base_type;

    typedef typename base_type::log_type            log_type;

    typedef typename base_type::rng_type            rng_type;
    typedef typename base_type::block_type          block_type;
    typedef typename base_type::allele_type         allele_type;
    typedef typename base_type::allele_set_type     allele_set_type;
    typedef typename base_type::sequence_type       sequence_type;
    typedef typename base_type::sequence_pointer    sequence_pointer;
    typedef typename base_type::individual_type     individual_type;

    typedef typename base_type::allele_generator    allele_generator;

    // recombination typedefs
    typedef typename base_type::classifier_type     classifier_type;
    typedef clotho::recombine::recombination< sequence_type, classifier_type, RECOMBINE_INSPECT_METHOD, BIT_WALK_METHOD >   recombination_engine_type;
    typedef recombiner< sequence_type, recombination_engine_type >               recombination_method;
    typedef clotho::utility::random_generator< rng_type, recombination_method >  recombination_method_generator;
    typedef typename base_type::population_type                                 population_type;
    typedef typename population_type::iterator                                  population_iterator;

    typedef clotho::utility::random_generator< rng_type, infinite_site< sequence_type > >       mutation_generator_type;
    typedef sequence_generator< sequence_pointer >                                              sequence_generator_type;
    typedef sequence_mutator< sequence_type, mutation_generator_type >                          sequence_mutator_type;
    typedef clotho::utility::random_generator< rng_type, sequence_mutator_type >                sequence_mutator_generator;
    typedef individual_initializer< individual_type, sequence_generator_type >   individual_initializer_type;
//    typedef individual_selector< rng_type >             individual_selector_type;
    typedef IND_SELECT < rng_type > individual_selector_type;

    typedef individual_reproduction< individual_type
    , sequence_mutator_generator
    , recombination_method_generator, REPRODUCTION_METHOD_TAG > individual_reproduction_type;

    typedef individual_generator< population_type, individual_selector_type, individual_reproduction_type >     individual_generator_type;

    typedef individual_resetter< individual_type >                  individual_resetter_type;
//    typedef individual_phenotyper< individual_type, no_type >       individual_phenotyper_type;
//
//    typedef std::vector< typename individual_phenotyper_type::result_type > population_phenotypes;

    typedef population_phenotyper< population_type >                population_phenotyper_type;
    typedef typename population_phenotyper_type::phenotype_type     phenotype_type;
    typedef typename population_phenotyper_type::result_type        population_phenotypes;

    // fitness typedefs
//    typedef clotho::fitness::fitness_method< double, clotho::fitness::multiplicative_heterozygous_tag > het_fit_type;
//    typedef clotho::fitness::fitness_method< double, clotho::fitness::multiplicative_homozygous_tag >    alt_hom_fit_type;
//    typedef clotho::fitness::no_fit                                             ref_hom_fit_type;
//    typedef clotho::fitness::fitness< sequence_type, het_fit_type, alt_hom_fit_type, ref_hom_fit_type, double > fitness_type;
//    typedef typename fitness_type::result_type                                  fitness_result_type;
//    typedef individual_fitness< fitness_type >                                  fitness_operator;
//
    typedef std::shared_ptr< ifitness_generator >                               fitness_generator_type;
    typedef std::shared_ptr< ifitness >                                         fitness_operator;
    typedef typename ifitness::result_type                                      fitness_result_type;
    typedef std::vector< fitness_result_type >                                  population_fitnesses;

    typedef std::shared_ptr< ipopulation_growth_generator >                     population_growth_generator_type;
    typedef std::shared_ptr< ipopulation_growth >                               population_growth_type;

    // statistic typedefs
    typedef std::map< sequence_pointer, unsigned int >  ref_map_type;
    typedef typename ref_map_type::iterator             ref_map_iterator;
    typedef std::vector< unsigned int >                 allele_dist_type;

    typedef EffectSizeMatrix < allele_set_type > effect_size_matrix_type;

    simulate_engine( boost::property_tree::ptree & config ) :
        qtl_logging_parameter( config )
        , m_rng()
        , m_founder_size( population_parameter::DEFAULT_POPULATION_SIZE )
        , m_seq_mut_gen( m_rng, config )
        , m_rec_met_gen( m_rng, config )
        , m_repro( m_seq_mut_gen, m_rec_met_gen )
        , m_parent( &m_pop )
        , m_child( &m_buffer )
        , m_parent_pheno( &m_pheno_buff1 )
        , m_child_pheno( &m_pheno_buff2 )
        , m_parent_fit( &m_fit_buff1 )
        , m_child_fit( &m_fit_buff2 )
        , m_pairwise_pop(false)
        , m_pop_grow()
        , m_eff_matrix( NULL )
    {
        parseConfig( config );
        initialize();
    }

    virtual ~simulate_engine() {
        delete m_eff_matrix;
    }

//    void simulate( unsigned int gen = 0) {
//            simulate( m_founder_size, gen );
//    }

//    void simulate( unsigned int p_size, unsigned int gen ) {
    void simulate( unsigned int gen ) {
        unsigned int p_size = m_founder_size;
        if( gen && m_pop_grow) {
            p_size = (*m_pop_grow)(m_parent->size(), gen);
        }

        individual_selector_type sel( m_rng, m_parent_fit->begin(), m_parent_fit->end() );
        individual_generator_type ind_gen( m_parent, sel, m_repro, gen );

        std::generate_n( std::back_inserter( *m_child ), p_size, ind_gen );

        m_child_pheno->reserve( m_child->size() );
        m_child_fit->reserve( m_child->size() );

        updatePhenotypes2( *m_child_pheno, *m_child );
        updateFitness( *m_child_fit, *m_child_pheno );
    }

    void reset_parent() {
        m_parent->clear();

        std::swap( m_parent, m_child );
        std::swap( m_parent_pheno, m_child_pheno );
        std::swap( m_parent_fit, m_child_fit );

        m_alleles.pruneSpace();

        m_child_pheno->clear();
        m_child_fit->clear();
    }

    population_type *   getParentPopulation() {
        return m_parent;
    }
    population_type *   getChildPopulation() {
        return m_child;
    }

    allele_set_type *   getAlleles() {
        return &m_alleles;
    }

    log_type &          getLog() {
        return m_log;
    }

    void                clearLog() {
        m_log.clear();
    }

    log_type            getState() {
        log_type state;

        log_type p, c, a;
//        state_of< population_type >::record( *m_parent, p );
//        state_of< population_type >::record( *m_child, c );
//        state_of< allele_set_type >::record( m_alleles, a );

        state.put( "population.parent", p );
        state.put( "population.child", c );

        state.put( "alleles", a);
        return state;
    }

    void computeStats( boost::property_tree::ptree & log ) {
        boost::property_tree::ptree pheno_log;
        BOOST_FOREACH( auto& v, *m_parent_pheno ) {
            clotho::utility::add_value_array( pheno_log, v );
        }

        log.add_child( "phenotypes", pheno_log);

        boost::property_tree::ptree fit_log;
        BOOST_FOREACH( auto& v, *m_parent_fit ) {
            clotho::utility::add_value_array( fit_log, v );
        }

        log.add_child( "fitness", fit_log );

        population_statistics( m_parent, &m_alleles, log, m_pairwise_pop );
        unsigned int i = 0;
//        BOOST_FOREACH( auto& v, m_sampling_sizes ) {
        BOOST_FOREACH( auto& v, m_sampling ) {
            boost::property_tree::ptree l;
            random_sampler( m_parent, m_parent_pheno, m_parent_fit, v.sample_size, l, v.pairwise );

            if( v.pairwise ) {
                boost::property_tree::ptree r;
                random_pairwise_stats( m_parent, 2 * v.sample_size, r);
                l.add_child("independent.pairwise", r);
            }

            l.put(SIZE_K, v.sample_size );

            std::ostringstream oss;
            oss << "sample." << (i++);

            log.add_child( oss.str(), l );
        }
    }

protected:

    void updatePhenotypes( population_phenotypes & phenos, population_type & p ) {
        bool all_neutral = true;
        for( typename allele_set_type::cvariable_iterator it = m_alleles.variable_begin(); all_neutral && it != m_alleles.variable_end(); ++it ) {
            all_neutral = it->isNeutral();
        }

        if( all_neutral ) {
//            typename population_phenotypes::value_type r;
//            while( phenos.size() < p.size() ) {
//                phenos.push_back( r );
//            }
            buildPhenotypesForNeutralTraits( phenos, p );
        } else {
            population_phenotyper_type ppheno;
            BOOST_FOREACH( auto& i, p ) {
                phenos.push_back( ppheno(i) );
            }
        }
    }

    void buildPhenotypesForNeutralTraits( population_phenotypes & phenos, population_type & p ) {
        while( phenos.size() < p.size() ) {
            typename population_phenotypes::value_type r( m_eff_matrix->m_trait_count, 0);
            phenos.push_back( r );
        }
    }

    void updatePhenotypes2( population_phenotypes & phenos, population_type & p ) {
//        bool all_neutral = true;
//        unsigned int allele_count = m_alleles.size();
//
//        typename allele_set_type::cvariable_iterator it = m_alleles.variable_begin();
//
//        // assume all alleles effect the same number of traits
//        // just use the first allele to determine this
//        unsigned int trait_count = it->trait_count();
//
//        typedef typename population_phenotypes::value_type::value_type effect_size_type;
//
//        m_eff_matrix.resize( allele_count, trait_count );
//
//        typename effect_size_matrix_type::value_type * eff_size_matrix = m_eff_matrix.m_eff_size_mat;
//
//        unsigned int i = 0;
//        while( it != m_alleles.variable_end() ) {
//            all_neutral = all_neutral && it->isNeutral();
//            for( typename allele_type::weight_citerator wit = it->begin(); wit != it->end(); wit++) {
//                effect_size_matrix[ i++ ] = *wit;
//            }
//            it++;
//        }

        bool all_neutral = m_eff_matrix->update( m_alleles );

        if( all_neutral ) {
            buildPhenotypesForNeutralTraits( phenos, p );
        } else {
            buildPhenotypes( phenos, p );
        }
    }

    void buildPhenotypes( population_phenotypes & phenos, population_type & p ) {
        population_iterator pit = p.begin();

        while( pit != p.end() ) {
            typename population_phenotypes::value_type ind_pheno( m_eff_matrix->m_trait_count, 0 );

            accum_sequence_phenotype( ind_pheno, (*pit->first) );
            accum_sequence_phenotype( ind_pheno, (*pit->second) );

            phenos.push_back( ind_pheno );
            ++pit;
        }
    }

    void accum_sequence_phenotype(typename population_phenotypes::value_type & ind_pheno, sequence_type & seq ) {
        typename effect_size_matrix_type::value_type * eff_size_matrix = m_eff_matrix->m_eff_size_mat;
        unsigned int traits = m_eff_matrix->m_trait_count;

        typename sequence_type::index_type idx = seq.find_first();
        while( idx.second != sequence_type::npos ) {
            unsigned int eff_size_idx = idx.first * traits;
            unsigned int i = 0;

            while( i < traits ) {
                ind_pheno[ i++ ] += eff_size_matrix[ eff_size_idx++ ];
            }

            idx = seq.find_next( idx );
        }
    }

    void updateFitness( population_fitnesses & fit, population_phenotypes & phenos ) {
//        fitness_operator pfit = m_fit_gen( phenos.begin(), phenos.end() );
        if( m_fit_gen ) {
            fitness_operator pfit = m_fit_gen->generate( phenos );
//            pfit->log( std::cerr );
            BOOST_FOREACH( auto& p, phenos ) {
                fit.push_back( pfit->operator()(p) );
            }
        } else {
            std::fill_n( std::back_inserter( fit ), phenos.size(), 1.);
        }
    }

    void parseConfig( boost::property_tree::ptree & config ) {
        seed_parameter<  > seed_param( config );
        m_rng.seed( seed_param.m_seed );

        population_parameter pop_param( config );
        m_founder_size = pop_param.m_size;

        m_fit_gen = fitness_toolkit::getInstance()->get_tool( config );
        if( m_fit_gen ) m_fit_gen->log( std::cerr );

        population_growth_generator_type tmp  = population_growth_toolkit::getInstance()->get_tool( config );
        if( tmp ) {
            m_pop_grow = tmp->generate();
            if( m_pop_grow ) {
                m_pop_grow->log( std::cerr );
                std::cerr << std::endl;
            }
        }
    }

//    void parseConfig( boost::property_tree::ptree & config ) {
//        std::ostringstream oss;
//        oss /*<< CONFIG_BLOCK_K << "."*/ << RNG_BLOCK_K << "." << SEED_K;
//
//        if( config.get_child_optional( oss.str() ) == boost::none ) {
//            config.put( oss.str(), 0 );
//        } else {
//            unsigned int seed = config.get< unsigned int >( oss.str(), 0 );
//            m_rng.seed( seed );
//        }
//
//        oss.str("");
//        oss.clear();
//
//        oss /*<< CONFIG_BLOCK_K << "." */<< POP_BLOCK_K << "." << SIZE_K;
//        if( config.get_child_optional( oss.str() ) == boost::none ) {
//            config.put( oss.str(), m_founder_size );
//        } else {
//            m_founder_size = config.get< unsigned int >(oss.str(), m_founder_size );
//        }
//
//        oss.str("");
//        oss.clear();
//
//        oss /*<< CONFIG_BLOCK_K << "."*/ << LOG_BLOCK_K << "." << SAMPLING_K;
//
//        if( config.get_child_optional( oss.str() ) == boost::none ) {
//            config.put( oss.str(), "" );
//        } else {
//            BOOST_FOREACH( auto& v, config.get_child( oss.str() ) ) {
////                std::ostringstream tmp;
////                tmp << v.second.data();
////
////                unsigned int s = boost::lexical_cast< unsigned int >( tmp.str() );
////                m_sampling_sizes.push_back(s);
//                sample_log_params sl(v.second);
//                m_sampling.push_back( sl );
//            }
//        }
//
//        oss.str("");
//        oss.clear();
//        oss << LOG_BLOCK_K << ".population_pairwise";
//
//        if( config.get_child_optional( oss.str() ) == boost::none ) {
//            config.put(oss.str(), m_pairwise_pop );
//        } else {
//            m_pairwise_pop = config.get< bool >( oss.str(), m_pairwise_pop);
//        }
//
//        m_fit_gen = fitness_toolkit::getInstance()->get_tool( config );
//        if( m_fit_gen ) m_fit_gen->log( std::cerr );
//
//        population_growth_generator_type tmp  = population_growth_toolkit::getInstance()->get_tool( config );
//        if( tmp ) {
//            m_pop_grow = tmp->generate();
//            if( m_pop_grow ) {
//                m_pop_grow->log( std::cerr );
//                std::cerr << std::endl;
//            }
//        }
//    }

    void initialize( ) {
        m_pop.clear();
        m_buffer.clear();

        m_pop.reserve( m_founder_size );
        m_buffer.reserve( m_founder_size );

        m_eff_matrix = new effect_size_matrix_type(0, 0);

        // results in the existence of multiple 'empty' sequences in the family
        sequence_generator_type sgen( m_alleles );
        individual_initializer_type igen( sgen );
        std::generate_n( std::back_inserter( m_pop ), m_founder_size, igen );

        //sequence_pointer empty_seq = m_alleles.create_subset();

        //std::fill_n( std::back_inserter( *m_parent ), m_founder_size, empty_seq );
        std::fill_n( std::back_inserter( *m_parent_pheno ), m_founder_size, phenotype_type(1, 0.) );
        updateFitness( *m_parent_fit, *m_parent_pheno );
    }

    void random_sampler( population_type * p, population_phenotypes * phenos, population_fitnesses * fits, unsigned int size, log_type & l, bool pairwise ) {
        population_type sub_pop;
        boost::property_tree::ptree pheno_log, fit_log;
        boost::random::uniform_int_distribution< unsigned int > ind_rand(0, p->size() - 1 );
        for( unsigned int i = 0; i < size; ++i ) {
            unsigned int idx = ind_rand( m_rng );
            sub_pop.push_back( p->at(idx) );
            clotho::utility::add_value_array( pheno_log, phenos->at(idx) );
            clotho::utility::add_value_array( fit_log, fits->at(idx) );
        }

        l.add_child( "phenotypes", pheno_log);
        l.add_child( "fitness", fit_log );

        population_statistics( &sub_pop, &m_alleles, l, pairwise );
    }

    void random_pairwise_stats( population_type * p, unsigned int size, boost::property_tree::ptree & l ) {
        ref_map_type seq_map;
        boost::random::uniform_int_distribution< unsigned int > ind_rand(0, 2 * p->size() - 1);
        while( size-- ) {
            unsigned int idx = ind_rand( m_rng );

            sequence_pointer tmp = ((idx % 2) ? (p->at(idx/2).second) : (p->at(idx/2).first));
            ref_map_iterator it = seq_map.find(tmp);
            if( it == seq_map.end() ) {
                seq_map.insert( std::make_pair( tmp, 1));
            } else {
                it->second += 1;
            }
        }

        allele_dist_type allele_dist( m_alleles.variable_allocated_size(), 0);
        unsigned int nAlleles = buildAlleleDistribution( seq_map, allele_dist);

        l.put("segregation_sites", nAlleles);

        pairwise_stats( seq_map, l);
    }

    void pairwise_stats( ref_map_type & rmap, boost::property_tree::ptree & l ) {
        typename pairwise_statistic< sequence_type >::accum_type global_diff, global_int, global_un;

        ref_map_iterator it = rmap.begin();

        std::vector< sequence_pointer > keys;
        for( ; it != rmap.end(); ++it ) {
            unsigned int c = it->second;
            while( c-- ) {
                keys.push_back(it->first);
            }
        }

        typename std::vector< sequence_pointer >::iterator kit = keys.begin();
        for( ; kit != keys.end(); ++kit ) {
            pairwise_statistic< sequence_type > pstat( **kit, global_diff, global_int, global_un, 1.0 );
            for( typename std::vector< sequence_pointer >::iterator  kit2 = kit + 1; kit2 != keys.end(); ++kit2 ) {
                pstat.update( **kit2, 1.0 );
            }
        }

        //l.put( "population.sequences.technical_duplicates", tech_dup );
        l.put( "sequences.pairwise.size", accum::count(global_int) );
        l.put( "sequences.pairwise.unique_pairs", accum::count(global_diff) );
        l.put( "sequences.pairwise.difference.mean", accum::weighted_mean(global_diff));
        l.put( "sequences.pairwise.difference.variance", accum::weighted_variance(global_diff));
        l.put( "sequences.pairwise.intersection.mean", accum::weighted_mean(global_int));
        l.put( "sequences.pairwise.intersection.variance", accum::weighted_variance(global_int));
        l.put( "sequences.pairwise.union.mean", accum::weighted_mean(global_un));
        l.put( "sequences.pairwise.union.variance", accum::weighted_variance(global_un));
    }

    /*    void pairwise_stats( ref_map_type & rmap, boost::property_tree::ptree & l ) {

            accum::accumulator_set< double, accum::stats< accum::tag::weighted_mean, accum::tag::weighted_variance >, double > acc_diff, acc_int, acc_un;

            unsigned int tech_dup = 0, pairs = 0;
            double tot = 0.;
            for( ref_map_iterator it = rmap.begin(); it != rmap.end(); ++it ) {
                sequence_pointer s0 = it->first;
                double w0 = (double)it->second;

                ref_map_iterator it2 = it;
                for( ++it2; it2 != rmap.end(); ++it2 ) {
                    sequence_pointer s1 = it2->first;
                    double w1 = (double)it2->second;
                    double _diff = 0, _int = 0, _un = 0;

    //                typename sequence_type::cblock_iterator sit = s0->begin(), sit2 = s1->begin();

                    typename sequence_type::data_citerator sit = s0->begin(), sit2 = s1->begin();

                    while( true ) {
                        if( sit == s0->end() ) {
                            while( sit2 != s1->end() ) {
                                block_type b = *sit2;
                                ++sit2;
                                double res = (double)popcount( b );
                                _diff += res;
                                _un += res;
                            }
                            break;
                        }

                        if( sit2 == s1->end() ) {
                            while( sit != s0->end() ) {
                                block_type b = *sit;
                                ++sit;
                                double res = (double)popcount( b );
                                _diff += res;
                                _un += res;
                            }
                            break;
                        }

                        block_type b0 = *sit, b1 = *sit2;
                        ++sit;
                        ++sit2;
                        _diff += popcount( b0 ^ b1 );
                        _int += popcount( b0 & b1 );
                        _un += popcount( b0 | b1 );
                    }

                    if( _diff > 0. ) {
                        double w = (w0 * w1);
                        acc_diff( _diff, accum::weight = w );
                        acc_int( _int, accum::weight = w );
                        acc_un( _un, accum::weight = w );
                        tot += w;
                        ++pairs;
                    } else {
                        // technical duplicate
                        ++tech_dup;
                    }
                }
            }

            l.put( "population.sequences.technical_duplicates", tech_dup );
            l.put( "sequences.pairwise.size", tot );
            l.put( "sequences.pairwise.unique_pairs", pairs );
            l.put( "sequences.pairwise.difference.mean", accum::weighted_mean(acc_diff));
            l.put( "sequences.pairwise.difference.variance", accum::weighted_variance(acc_diff));
            l.put( "sequences.pairwise.intersection.mean", accum::weighted_mean(acc_int));
            l.put( "sequences.pairwise.intersection.variance", accum::weighted_variance(acc_int));
            l.put( "sequences.pairwise.union.mean", accum::weighted_mean(acc_un));
            l.put( "sequences.pairwise.union.variance", accum::weighted_variance(acc_un));
        }*/

//    void buildRefMap( population_type * p, ref_map_type & m, allele_dist_type & a ) {
    void buildRefMap( population_type * p, ref_map_type & m ) {

        population_iterator pit = p->begin();
        while( pit != p->end() ) {
            ref_map_iterator rit = m.find( pit->first );
            if( rit == m.end() ) {
                m.insert( std::make_pair( pit->first, 1 ));
            } else {
                ++(rit->second);
            }

            rit = m.find( pit->second);
            if( rit == m.end() ) {
                m.insert( std::make_pair( pit->second, 1 ) );
            } else {
                ++(rit->second);
            }
            ++pit;
        }

//        buildAlleleDistribution(m,a);
    }

    unsigned int buildAlleleDistribution( ref_map_type & m, allele_dist_type & a) {
        unsigned int nAlleles = 0;
        for( ref_map_iterator rit = m.begin(); rit != m.end(); ++rit ) {
            unsigned int n = rit->second;
            typename sequence_type::index_type idx = rit->first->find_first();
            while( idx.second != sequence_type::npos ) {
                //std::cerr << idx.first << "," << idx.second << "," << sequence_type::npos << std::endl;
                if( a[idx.first] == 0 ) ++nAlleles;
                a[ idx.first ] += n;
                idx = rit->first->find_next( idx );
            }
        }
        return nAlleles;
    }

    void population_statistics( population_type * p, allele_set_type * alleles, boost::property_tree::ptree & _log, bool pairwise ) {

        typedef std::map< unsigned int, unsigned int >  count_map;
        typedef count_map::iterator                     count_iterator;

        count_map allele_counts;

        ref_map_type seq_ref_counts;
        buildRefMap(p, seq_ref_counts );

        unsigned int nExpSeq = 0;
        accum::accumulator_set< double, accum::stats< accum::tag::weighted_mean, accum::tag::weighted_median, accum::tag::weighted_variance >, double > acc;
        BOOST_FOREACH( auto& v, seq_ref_counts ) {
            double acnt = (( v.first ) ? v.first->count() : 0 );
            double e_rcnt = v.second;

            count_iterator a_it = allele_counts.find( acnt );
            if( a_it == allele_counts.end() ) {
                allele_counts.insert( std::make_pair( acnt, e_rcnt ) );
            } else {
                a_it->second += e_rcnt;
            }

            acc( acnt, accum::weight = e_rcnt );
            nExpSeq += e_rcnt;
        }

        double ave_alleles_per_sequence = accum::weighted_mean(acc);
        double med_allele = accum::weighted_median( acc );
        double var_allele = accum::weighted_variance( acc );

        boost::property_tree::ptree all_dist, all_freq;
        for( count_iterator c_it = allele_counts.begin(); c_it != allele_counts.end(); ++c_it) {
            clotho::utility::add_value_array( all_dist, *c_it );
        }

        allele_dist_type allele_dist( alleles->variable_allocated_size(), 0);
        unsigned int nSites = buildAlleleDistribution(seq_ref_counts, allele_dist);

        _log.put("segregation_sites", nSites );

        unsigned int n=0;
        typename allele_set_type::cvariable_iterator e_it = alleles->variable_begin();
        for( allele_dist_type::iterator a_it = allele_dist.begin(); a_it != allele_dist.end(); ++a_it, ++e_it ) {
            if( (*a_it) > 0 ) {
                std::ostringstream oss;
                oss << *e_it;
                clotho::utility::add_value_array( all_freq, std::make_pair(oss.str(), (*a_it)));
                ++n;
            } else {
                clotho::utility::add_value_array( all_freq, "");
            }
        }

        assert( n == nSites );

        if (pairwise) {
            pairwise_stats( seq_ref_counts, _log);
        }

        //_log.put( "population.family_size", seq_ref_counts.size() );
        //_log.put( "population.reference_sequences", ((allele_counts.find(0) != allele_counts.end())? allele_counts[0] : 0 ) );
        //_log.put( "population.total_sequences", nExpSeq );
        //
        _log.put( "population.sequences.unique", seq_ref_counts.size() );
        _log.put( "population.sequences.num_empty", ((allele_counts.find(0) != allele_counts.end() ) ? allele_counts[0] : 0));
        _log.put( "population.size", p->size() );

        _log.put( "sequences.allele_count_distribution.format", "[alleles per sequence, number of sequences in population]" );
        _log.add_child( "sequences.allele_count_distribution.value", all_dist );
        _log.put( "sequences.alleles_per.mean", ave_alleles_per_sequence);
        _log.put( "sequences.alleles_per.min", allele_counts.begin()->first );

        count_iterator bs = allele_counts.begin();
        unsigned int i = 1;
        while( i++ < allele_counts.size() ) { ++bs; }

        //_log.put( "sequences.alleles_per.max", allele_counts.crbegin()->first );
        //_log.put( "sequences.alleles_per.max", (allele_counts.begin() + allele_counts.size() )->first);
        _log.put( "sequences.alleles_per.max", bs->first );
        _log.put( "sequences.alleles_per.median", med_allele );
        _log.put( "sequences.alleles_per.variance", var_allele );

        _log.put( "alleles.variable_count", alleles->variable_size() );
        _log.put( "alleles.fixed_count", alleles->fixed_size() );
        _log.put( "alleles.free_size", alleles->free_size() );

        _log.put( "alleles.frequencies.format", "[allele, count in population]" );
        _log.add_child( "alleles.frequencies.value", all_freq );
    }

    rng_type        m_rng;
    unsigned int    m_founder_size;
    sequence_mutator_generator m_seq_mut_gen;
    recombination_method_generator m_rec_met_gen;

    individual_reproduction_type    m_repro;
    fitness_generator_type          m_fit_gen;

    log_type            m_log;
    population_type     m_pop, m_buffer;
    population_type     * m_parent, * m_child;

    population_phenotypes   m_pheno_buff1, m_pheno_buff2;
    population_phenotypes   * m_parent_pheno, * m_child_pheno;

    population_fitnesses m_fit_buff1, m_fit_buff2;
    population_fitnesses * m_parent_fit, * m_child_fit;

    allele_set_type m_alleles;

//    std::vector< unsigned int > m_sampling_sizes;
//    std::vector< sample_log_params > m_sampling;

    bool m_pairwise_pop;

    population_growth_type m_pop_grow;

    effect_size_matrix_type * m_eff_matrix;
};

namespace clotho {
namespace utility {

template < class URNG, class AlleleType, class LogType, class TimerType >
struct parameter_space< simulate_engine< URNG, AlleleType, LogType, TimerType > > {

    static void build_parameters( boost::property_tree::ptree & params ) {
        std::ostringstream oss;
        oss /*<< CONFIG_BLOCK_K << "."*/ << POP_BLOCK_K << "." << SIZE_K;
        params.put( oss.str() + ".value", 1000 );
        params.put( oss.str() + ".type", "uint");
        params.put( oss.str() + ".description", "Founder Population Size" );
    }
};

}   // namespace utility
}   // namespace clotho

#endif  // SIMULATE_ENGINE_HPP_


#include <ctime>
#include <cstdio>
#include <cstdlib>
#include <vector>
#include <iostream>
#include <boost/date_time.hpp>

#include "kdl_parser/kdl_parser.hpp"
#include "kdl/chain.hpp"

#include "tools.hpp"
#include "hls_ik.hpp"

float fRand(float min, float max)
{
    float f = (float)rand() / RAND_MAX;
    return min + f * (max - min);
}

int test(int num_samples, std::string chain_start, std::string chain_end, float eps, std::string urdf_file)
{
    srand(time(NULL));

    KDL::Tree tree;
    if (!kdl_parser::treeFromFile(urdf_file, tree)) {
        printf("Failed to extract kdl tree from xml robot description\n");
        return 1;
    }

    KDL::Chain chain;
    if(!tree.getChain(chain_start, chain_end, chain)) {
        printf("Couldn't find chain %s to %s\n",chain_start.c_str(),chain_end.c_str());
        return 2;
    }

    if (chain.getNrOfJoints() != chain.getNrOfSegments()) {
        printf("Do not support this type of robot for now\n");
        return 3;
    }

    // int N = chain.getNrOfJoints();
    int rc;
    int success;
    int success_steps;
    float total_time;
    float success_time;
    float end_twist[6];
    float end_pose[12];
    float end_pose_now[12];
    float end_pose_test[12];
    float jlist_in[MAX_DEGREE];
    float jlist_result[MAX_DEGREE];
    float JointList[num_samples][MAX_DEGREE];
    boost::posix_time::ptime start_time;
    boost::posix_time::time_duration diff;

    // Set up HLS_IK
    float hls_chain[MAX_DEGREE][18];
    bool hls_joint_type[MAX_DEGREE];
    int ttmp = 0;
    for (int i = 0; i < N; i++) {
        for (int j = 0; j < 9; j++) {
            hls_chain[i][j] = chain.getSegment(i).getFrameToTip().M.data[j];
        }
        hls_chain[i][9] = 0;
        hls_chain[i][10] = 0;
        hls_chain[i][11] = 0;
        for (int j = 0; j < 3; j++) {
            hls_chain[i][j+12] = chain.getSegment(i).getJoint().JointAxis().data[j];
            hls_chain[i][j+15] = chain.getSegment(i).getJoint().JointOrigin().data[j];
        }
        hls_joint_type[i] = chain.getSegment(i).getJoint().getType() == 0 ? 0 : 1;
        ttmp += hls_joint_type[i];
    }
    hls_ik(N, 1000, eps, hls_chain, hls_joint_type, jlist_in, end_pose, jlist_result);

    printf("ttmp = %d\n", ttmp);
    printf("N = %d\n", N);

    // Set up TOOLS
    TOOLS::TOOLS tools(hls_chain, hls_joint_type, N);

    // Set up init joint list
    for (int j = 0; j < N; j++) {
        jlist_in[j] = 0;
    }

    // Set up test joint list
    for (int i = 0; i < num_samples; i++) {
        for (int j = 0; j < N; j++) {
            JointList[i][j] = fRand(-1, 1);
        }
    }

    total_time=0;
    success=0;
    success_time = 0;
    success_steps = 0;

    printf("*** Testing HLS_IK with %d random samples, Using %d joints\n", num_samples, N);

    for (int i = 0; i < num_samples; i++) {
        tools.my_fk_solver(JointList[i], end_pose);

        // hls fk algorithm test
        // hls_solver.my_fk_solver(jlist, end_pose);
        // hls_ik(2, N, 1000, eps, hls_chain, hls_joint_type, JointList[i], end_pose_test, jlist_result);

        // for (int j = 0; j < 12; j++)
        //     printf("%f ", end_pose[j]);
        // printf("\n");
        // for (int j = 0; j < 12; j++)
        //     printf("%f ", end_pose_test[j]);
        // printf("\n");
        // bool is_same = 1;
        // for (int j = 0; j < 12; j++)
        //     if (fabs(end_pose_test[j] - end_pose[j]) > eps)
        //         is_same = 0;
        // if (!is_same)
        //     printf("No!!!!!!! fk hls\n");

        start_time = boost::posix_time::microsec_clock::local_time();
        rc = hls_ik(N, 1000, eps, hls_chain, hls_joint_type, jlist_in, end_pose, jlist_result);
        diff = boost::posix_time::microsec_clock::local_time() - start_time;

        // validity
        tools.my_fk_solver(jlist_result, end_pose_now);
        tools.diff(end_pose_now, end_pose, end_twist);
        if (rc >= 0 && fabs(end_twist[0]) < eps && fabs(end_twist[1]) < eps && fabs(end_twist[2]) < eps && fabs(end_twist[3]) < eps && fabs(end_twist[4]) < eps && fabs(end_twist[5]) < eps) {
            success++;
            success_steps += rc;
            success_time += diff.total_nanoseconds() / 1e9;
        } else {
            printf("test %d fail\n", i);
            for (int j = 0; j < 6; j++)
                printf("%f ", end_twist[j]);
            printf("\n");
        }

        if (i*100/num_samples%10 == 0)
            printf("%d%% done\n", int(float(i)/num_samples*100));
    }

    printf("HLS_IK success rate:             %f%%\n", 100.0*success/num_samples);
    printf("HLS_IK success average steps:    %f\n", success_steps/(success+1e-10));
    printf("HLS_IK success average time:     %fs\n", success_time/(success+1e-10));

    printf("Test done\n");

    return 0;
}

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

    int num_samples = 100;
    float eps = 0.00001;

    std::string chain_start = "branch_0";
    std::string chain_end = "branch_100";
    std::string urdf_file = "/home/pnpeasy/Workspace/vivado/ik/hls/urdf/d_0100_1.urdf";

    return test(num_samples, chain_start, chain_end, eps, urdf_file);
}


// Copyright 2013 The CARFAC Authors. All Rights Reserved.
// Author: Kevin Wilson <kwwilson@google.com>
//
// This file is part of an implementation of Lyon's cochlear model:
// "Cascade of Asymmetric Resonators with Fast-Acting Compression"
//
// 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 "binaural_sai.h"

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

#include <Eigen/Core>

#include "gtest/gtest.h"

#include "agc.h"
#include "car.h"
#include "carfac.h"
#include "common.h"
#include "ihc.h"
#include "test_util.h"

using testing::Values;

class BinauralSAIPeriodicInputTest
    : public SAITestBase,
      public ::testing::WithParamInterface< ::testing::tuple<int, int> > {
 protected:
  void SetUp() {
    period_ = ::testing::get<0>(GetParam());
    num_channels_ = ::testing::get<1>(GetParam());
  }

  int period_;
  int num_channels_;
};

TEST_P(BinauralSAIPeriodicInputTest, MultiChannelPulseTrain) {
  const int kInputSegmentWidth = 38;
  const int kLeftEarDelay = 0;
  const int kRightEarDelay = 1;
  std::vector<ArrayXX> input_segment;
  // Arbitrarily refer to ear 0 as the left ear.
  input_segment.push_back(CreatePulseTrain(num_channels_, kInputSegmentWidth,
                                           period_, kLeftEarDelay));
  input_segment.push_back(CreatePulseTrain(num_channels_, kInputSegmentWidth,
                                           period_, kRightEarDelay));

  const int kSAIWidth = 15;
  const int kTriggerWindowWidth = kInputSegmentWidth;
  SAIParams sai_params = CreateSAIParams(num_channels_, kInputSegmentWidth,
                                         kTriggerWindowWidth, kSAIWidth);
  sai_params.future_lags = 2;

  BinauralSAI binaural_sai(sai_params);
  std::vector<ArrayXX> output_frame;
  binaural_sai.RunSegment(input_segment, &output_frame);

  // The output should have peaks at the same positions across all
  // frequency channels, regardless of input phase.  Output peaks
  // should differ from one "ear" to the other based on the delay
  // difference between ears.
  for (int ear = 0; ear < 2; ++ear) {
    // The peaks in the two ears should be offset from the zero-lag
    // index by a delay equal in magnitude to the relative delay
    // between the ears, positive offset in one ear, negative in the
    // other.
    int ear_offset = sai_params.future_lags + 1 +
                     (ear == 0 ? 1 : -1) * (kLeftEarDelay - kRightEarDelay);
    const ArrayXX& ear_frame = output_frame[ear];
    for (int i = 0; i < num_channels_; ++i) {
      const ArrayX& sai_channel = ear_frame.row(i);
      for (int j = sai_channel.size() - ear_offset; j >= 0; j -= period_) {
        EXPECT_TRUE(HasPeakAt(sai_channel, j)) << "ear: " << ear << " i: " << i
                                               << " j: " << j;
      }
    }
    std::cout << "Input " << ear << ":" << std::endl << input_segment[ear]
              << std::endl << "Output " << ear << ":" << std::endl
              << output_frame[ear] << std::endl;
  }
}

INSTANTIATE_TEST_SUITE_P(PeriodicInputVariations, BinauralSAIPeriodicInputTest,
                         testing::Combine(Values(25, 10, 5, 2),  // periods.
                                          Values(1, 2, 15)));  // num_channels.

class BinauralSAITest : public SAITestBase {};

TEST_F(BinauralSAITest, DiesIfNotTwoEars) {
  const int kNumChannels = 2;
  const int kInputSegmentWidth = 10;
  const int kPeriod = 4;
  std::vector<ArrayXX> input_segment;
  input_segment.push_back(
      CreatePulseTrain(kNumChannels, kInputSegmentWidth, kPeriod, 0));

  const int kSAIWidth = 20;
  const int kTriggerWindowWidth = kSAIWidth + 1;
  SAIParams sai_params = CreateSAIParams(kNumChannels, kInputSegmentWidth,
                                         kTriggerWindowWidth, kSAIWidth);
  BinauralSAI binaural_sai(sai_params);
  std::vector<ArrayXX> output_frame;
  // Dies on one ear.
  ASSERT_DEATH(binaural_sai.RunSegment(input_segment, &output_frame),
               "input_segment.size()");

  input_segment.push_back(
      CreatePulseTrain(kNumChannels, kInputSegmentWidth, kPeriod, 0));
  input_segment.push_back(
      CreatePulseTrain(kNumChannels, kInputSegmentWidth, kPeriod, 0));
  // Dies on three ears.
  ASSERT_DEATH(binaural_sai.RunSegment(input_segment, &output_frame),
               "input_segment.size()");
}


#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE GurobiConverterTests
#include <boost/test/unit_test.hpp>

#include "math/GurobiConverter.hpp"
#include "io/PBQP_Serializer.hpp"
#include "debug/DebugTimer.hpp"
#include <string>

namespace pbqppapa {

//we need to at least have an empty test case if gurobi is not enabled, otherwise the test suite will fail entirely
BOOST_AUTO_TEST_CASE(empty) {
#if PBQP_USE_GUROBI
	PBQP_Serializer<InfinityWrapper<unsigned int>> serial;
	std::vector<std::string> paths;
	paths.push_back("test/testData/smallLibfirmGraph.json");
	paths.push_back("test/testData/problematic.json");
	std::cout << "Linear solving: " << '\n';
	for (std::string path : paths) {
		std::cout << "Processing " << path << '\n';
		DebugTimer timer("Total timer: " + path);
		PBQPGraph<InfinityWrapper<unsigned int>>* ogGraph = serial.loadFromFile(
				path);
		GurobiConverter<unsigned int> gConv(ogGraph);
		timer.startTimer();
		PBQPSolution<InfinityWrapper<unsigned int>>* solution =
				gConv.solveGurobiLinear();
		timer.stopTimer();
		std::cout << timer.getOutput(true)
				<< std::to_string(solution->getTotalCost(ogGraph).getValue())
				<< '\n';
		delete solution;
	}
	std::cout << "Quadratic solving: " << '\n';
	for (std::string path : paths) {
		std::cout << "Processing " << path << '\n';
		DebugTimer timer("Total timer: " + path);
		PBQPGraph<InfinityWrapper<unsigned int>>* ogGraph = serial.loadFromFile(
				path);
		GurobiConverter<unsigned int> gConv(ogGraph);
		timer.startTimer();
		PBQPSolution<InfinityWrapper<unsigned int>>* solution =
				gConv.solveGurobiQuadratic();
		timer.stopTimer();

		std::cout << timer.getOutput(true)
				<< std::to_string(solution->getTotalCost(ogGraph).getValue())
				<< '\n';
		delete solution;
	}
#endif
}
}


#include <boost/geometry/strategies/spherical/distance_haversine.hpp>


/**
 * @brief
 *
 * @file barra.hpp
 * @author Luiz Victor Linhares Rocha
 * @date 2018-09-22
 * @copyright 2018
 */

#ifndef REDE_BARRA_HPP_
#define REDE_BARRA_HPP_

#define USE_MATH_DEFINES
#include <complex>
#include <cmath>
#include <vector>
#include <boost/shared_ptr.hpp>
#include "neuralFluxDeclare.hpp"
#include "../util/complexutilsDeclare.h"
#include "../util/connection.hpp"
#include "../util/connector.hpp"
#include "../util/identifiable.hpp"
#include "../util/changeTrackable.hpp"

namespace neuralFlux {

class Bar : public ConnectionPoint<Branch>, public Identifiable, public ChangeTrackable {
 public:
    virtual ~Bar();
    enum barType{ Slack = 1, PV = 2 , PQ = 3 };
    // bar factories with standard values
    static const BarPtr newSlack(
        const double v,
        const double teta,
        const uint64_t id = 0,
        const double p = 0,
        const double q = 0,
        const double bsh = 0);

    static const BarPtr newPV(
        const double p,
        const double v,
        const uint64_t id = 0,
        const double teta = 0,
        const double q = 0,
        const double bsh = 0);

    static const BarPtr newPQ(
        const double p,
        const double q,
        const uint64_t id = 0,
        const double v = 1,
        const double teta = 0,
        const double bsh = 0);

    static const BarPtr newBar(
        barType type,
        uint64_t id,
        double v = 1,
        double teta = 0,
        double p = 0,
        double q = 0,
        double bsh = 0);

    ///* getters and setters*/
    const complexo getE() const;
    const complexo getS() const;
    double getP() const;
    double getQ() const;
    double getV() const;
    double getTeta() const;
    barType getType() const;
    double getBsh() const;

    void setE(const complexo e);
    void setS(const complexo s);
    void setP(const double p);
    void setQ(const double q);
    void setV(const double v);
    void setTeta(const double teta);
    void setBsh(const double bsh);
    void setType(barType type);

 private:
    complexo m_e;
    complexo m_s;
    double m_bsh;
    barType m_type;
    Bar(const barType type,
        const double v,
        const double teta,
        const double p,
        const double q,
        const uint64_t id = 0,
        const double bsh = 0);
};
}  // namespace neuralFlux

#endif  // REDE_BARRA_HPP_


/*
Copyright 2009-2021 Nicolas Colombe

Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*/

#include <engine/pathfinding/navmesh.hpp>
#include <boost/graph/connected_components.hpp>
#include <engine/common/graphutils.hpp>
#include <math/mathtools.hpp>
#include <boost/graph/astar_search.hpp>
#include <engine/pathfinding/penumbratools.hpp>

namespace eXl
{
  Vector2f NavMesh::Face::GetTreadmillDir(Vector2f const& iPos) const
  {
    Vector2f faceCenter = m_Box.GetCenter();
    Vector2f halfSize = m_Box.GetSize() * 0.5;
    Vector2f otherCorner(halfSize.X(), -halfSize.Y());
    Vector2f relPos = iPos - faceCenter;

    static const Vector2f treadMillDirs[] =
    {
      -Vector2f::UNIT_Y,
       Vector2f::UNIT_X,
       Vector2f::UNIT_Y,
      -Vector2f::UNIT_X
    };

    uint32_t quadrant;
    if (Segmentf::IsLeft(-halfSize, halfSize, relPos) > 0)
    {
      if (Segmentf::IsLeft(-otherCorner, otherCorner, relPos) > 0)
      {
        quadrant = 3;
      }
      else
      {
        quadrant = 0;
      }
    }
    else
    {
        if (Segmentf::IsLeft(-otherCorner, otherCorner, relPos) > 0)
        {
          quadrant = 2;
        }
        else
        {
          quadrant = 1;
        }
    }

    if (m_HasSide[quadrant])
    {
      return treadMillDirs[quadrant];
    }

    int32_t axis = quadrant % 2;
    int32_t otherAxis = (1 - axis);
    int32_t sign = relPos.m_Data[otherAxis] > 0 ? 1 : 0;
    quadrant = otherAxis + sign * 2;
    if (m_HasSide[quadrant])
    {
      return treadMillDirs[quadrant];
    }

    return MathTools::GetPerp(relPos);
  };

  template <typename Real>
  boost::optional<Segment<Real>> Touch(AABB2D<Real> const& iBox1, AABB2D<Real> const& iBox2)
  {
    Segment<Real> res;

    if(iBox2.m_Data[1].Y() - iBox1.m_Data[0].Y() >= Math<Real>::ZERO_TOLERANCE && iBox1.m_Data[1].Y() - iBox2.m_Data[0].Y() >= Math<Real>::ZERO_TOLERANCE)
    {
      res.m_Ext1.Y() = Math<Real>::Max(iBox1.m_Data[0].Y(), iBox2.m_Data[0].Y());
      res.m_Ext2.Y() = Math<Real>::Min(iBox1.m_Data[1].Y(), iBox2.m_Data[1].Y());

      Real val = iBox1.m_Data[0].X() - iBox2.m_Data[1].X();
      if(val <= Math<Real>::ZERO_TOLERANCE && val >= -Math<Real>::ZERO_TOLERANCE)
      {
        res.m_Ext1.X() = res.m_Ext2.X() = iBox1.m_Data[0].X();
        return res;
      }

      val = iBox1.m_Data[1].X() - iBox2.m_Data[0].X();
      if(val <= Math<Real>::ZERO_TOLERANCE && val >= -Math<Real>::ZERO_TOLERANCE)
      {
        res.m_Ext1.X() = res.m_Ext2.X() = iBox1.m_Data[1].X();
        return res;
      }
    }

    if(iBox2.m_Data[1].X() - iBox1.m_Data[0].X() >= Math<Real>::ZERO_TOLERANCE && iBox1.m_Data[1].X() - iBox2.m_Data[0].X() >= Math<Real>::ZERO_TOLERANCE)
    {
      res.m_Ext1.X() = Math<Real>::Max(iBox1.m_Data[0].X(), iBox2.m_Data[0].X());
      res.m_Ext2.X() = Math<Real>::Min(iBox1.m_Data[1].X(), iBox2.m_Data[1].X());

      Real val = iBox1.m_Data[0].Y() - iBox2.m_Data[1].Y();
      if(val <= Math<Real>::ZERO_TOLERANCE && val >= -Math<Real>::ZERO_TOLERANCE)
      {
        res.m_Ext1.Y() = res.m_Ext2.Y() = iBox1.m_Data[0].Y();
        return res;
      }

      val = iBox1.m_Data[1].Y() - iBox2.m_Data[0].Y();
      if(val <= Math<Real>::ZERO_TOLERANCE && val >= -Math<Real>::ZERO_TOLERANCE)
      {
        res.m_Ext1.Y() = res.m_Ext2.Y() = iBox1.m_Data[1].Y();
        return res;
      }
    }

    return boost::none;
  }

  NavMesh NavMesh::MakeFromAABB2DPoly(Vector<AABB2DPolygoni> const& iPolys)
  {
    Vector<AABB2Di> boxes;

    for(auto& poly : iPolys)
    {
      Vector<AABB2Di> polyBoxes;
      poly.GetBoxes(polyBoxes);
      boxes.insert(boxes.end(), polyBoxes.begin(), polyBoxes.end());
    }

    return MakeFromBoxes(boxes);
  }

  NavMesh NavMesh::MakeFromBoxes(Vector<AABB2Di> const& iBoxes)
  {
    BoxIndex index;
    Vector<Face> tempFaces;
    EdgeMap tempEdges;
    Vector<Vector<std::pair<uint32_t, uint32_t>>> edgeConnectivity;

    for(auto const& box : iBoxes)
    {
      auto boxF = AABB2Df(MathTools::ToFVec(box.m_Data[0]), MathTools::ToFVec(box.GetSize()));
      auto value = std::make_pair(boxF, (unsigned int)index.size());
      index.insert(value);
      tempFaces.push_back(Face());
      tempFaces.back().m_Box = boxF;
    }

    Graph tempGraph;
    edgeConnectivity.resize(tempFaces.size());
    //for(unsigned int i = 0; i<index.size(); ++i)
    //{
    //  boost::add_vertex(tempGraph);
    //}

    for(auto& boxEntry : index)
    {
      Vector<BoxIndexEntry> results;

      AABB2Df queryBox = boxEntry.first;
      queryBox.m_Data[0] -= Vector2f::ONE;
      queryBox.m_Data[1] += Vector2f::ONE;

      index.query(
        /*boost::geometry::index::satisfies([&box](BoxIndexEntry const& iEntry)
          {
            iEntry.first != box.first && iEntry.first.Touch(box.first) != 0;
          }*/
        boost::geometry::index::intersects(queryBox),
        std::back_inserter(results));

      for(auto& result : results)
      {
        if(result != boxEntry)
        {
          auto res = Touch(result.first, boxEntry.first);
          if(res)
          {
            if(boxEntry.second > result.second)
            {
              Segmentf seg;
              seg.m_Ext1 = MathTools::ToFVec(res->m_Ext1);
              seg.m_Ext2 = MathTools::ToFVec(res->m_Ext2);
              auto newVtx = boost::add_vertex(tempGraph);

              Edge newEdge =
              {
                result.second,
                boxEntry.second,
                seg
              };
              eXl_ASSERT(tempEdges.size() == newVtx);
              tempEdges.push_back(newEdge);
              edgeConnectivity[boxEntry.second].push_back(std::make_pair(result.second, (uint32_t)newVtx));
            }
          }
        }
      }
    }

    for(unsigned int faceIdx = 0; faceIdx<tempFaces.size(); ++faceIdx)
    {
      auto& edges = edgeConnectivity[faceIdx];
      for(auto const& conn : edges)
      {
        tempFaces[faceIdx].m_Edges.push_back(conn.second);
        tempFaces[conn.first].m_Edges.push_back(conn.second);
      }
    }

    for(auto& face : tempFaces)
    {
      auto const& edges = face.m_Edges;
      for(unsigned int i = 0; i<edges.size(); ++i)
      {
        for(unsigned int j = i; j<edges.size(); ++j)
        {
          boost::add_edge(edges[i], edges[j], tempGraph);
        }
      }
    }

    if(boost::num_edges(tempGraph) == 0)
    {
      //Limited to a single face.
      NavMesh result;
      result.m_Components.push_back(Component());

      Component& resMesh = result.m_Components.back();

      resMesh.m_Graph = tempGraph;
      resMesh.m_Faces.push_back(Face());

      Face& face = resMesh.m_Faces.back();
      face.m_Box = tempFaces[0].m_Box;

      Vector2f faceDim = face.m_Box.GetSize();

      Vector2f corners[] =
      {
        face.m_Box.m_Data[0],
        face.m_Box.m_Data[0] + faceDim.X() * Vector2f::UNIT_X,
        face.m_Box.m_Data[0] + faceDim.Y() * Vector2f::UNIT_Y,
        face.m_Box.m_Data[0] + faceDim.X() * Vector2f::UNIT_X + faceDim.Y() * Vector2f::UNIT_Y
      };

      face.m_Walls.push_back(Segmentf({corners[0], corners[1]}));
      face.m_Walls.push_back(Segmentf({corners[0], corners[2]}));
      face.m_Walls.push_back(Segmentf({corners[3], corners[1]}));
      face.m_Walls.push_back(Segmentf({corners[3], corners[2]}));
      for (auto& val : face.m_HasSide) { val = true; }

      resMesh.m_FacesIdx.insert(std::make_pair(tempFaces[0].m_Box, 0));

      return result;
    }

    IndexMap<Graph> componentMap;
    unsigned int numComponents = boost::connected_components(tempGraph, componentMap);

    Vector<uint32_t> numVtx(numComponents, 0);
    Vector<uint32_t> numFaces(numComponents, 0);
    Vector<uint32_t> newId(boost::num_vertices(tempGraph), 0);
    Vector<uint32_t> newFaceId(tempFaces.size(), UINT32_MAX);
    Vector<Component> resComps(numComponents);

    for(uint32_t vtxId = 0; vtxId < tempEdges.size(); ++vtxId)
    {
      uint32_t compNum = componentMap.m_Map[vtxId];

      newId[vtxId] = numVtx[compNum]++;

      auto& curRes = resComps[compNum];

      unsigned int newVtxId = boost::add_vertex(curRes.m_Graph);
      auto const& edgeDesc = tempEdges[vtxId];

      if(newFaceId[edgeDesc.face1] == UINT32_MAX)
      {
        newFaceId[edgeDesc.face1] = numFaces[compNum]++;
        curRes.m_Faces.push_back(Face());
        curRes.m_Faces.back().m_Box = tempFaces[edgeDesc.face1].m_Box;
        curRes.m_FacesIdx.insert(std::make_pair(curRes.m_Faces.back().m_Box, newFaceId[edgeDesc.face1]));
      }

      curRes.m_Faces[newFaceId[edgeDesc.face1]].m_Edges.push_back(newVtxId);

      if(newFaceId[edgeDesc.face2] == UINT32_MAX)
      {
        newFaceId[edgeDesc.face2] = numFaces[compNum]++;
        curRes.m_Faces.push_back(Face());
        curRes.m_Faces.back().m_Box = tempFaces[edgeDesc.face2].m_Box;
        curRes.m_FacesIdx.insert(std::make_pair(curRes.m_Faces.back().m_Box, newFaceId[edgeDesc.face2]));
      }

      curRes.m_Faces[newFaceId[edgeDesc.face2]].m_Edges.push_back(newVtxId);

      Edge newEdgeDesc =
      {
        newFaceId[edgeDesc.face1],
        newFaceId[edgeDesc.face2],
        edgeDesc.segment
      };

      eXl_ASSERT(newVtxId == curRes.m_FaceEdges.size());
      curRes.m_FaceEdges.push_back(newEdgeDesc);
    }

    for(auto vertices = boost::vertices(tempGraph); vertices.first != vertices.second; ++vertices.first)
    {
      unsigned int curVtx = *vertices.first;
      unsigned int compNum = componentMap.m_Map[curVtx];
      unsigned int newCurVtx = newId[curVtx];

      auto& curRes = resComps[compNum];

      for(auto edges = boost::out_edges(curVtx, tempGraph); edges.first != edges.second; ++edges.first)
      {
        auto otherVtx = edges.first->m_source == curVtx ? edges.first->m_target : edges.first->m_source;
        unsigned int newOtherVtx = newId[otherVtx];
        auto edgeId = boost::add_edge(newCurVtx, newOtherVtx, curRes.m_Graph);

        auto const& seg1 = curRes.m_FaceEdges[curVtx];
        auto const& seg2 = curRes.m_FaceEdges[otherVtx];

        Vector2f vec = (seg1.segment.m_Ext1 + seg1.segment.m_Ext2) * 0.5 - (seg2.segment.m_Ext1 + seg2.segment.m_Ext2) * 0.5;
        boost::property_map<Graph, boost::edge_weight_t>::type propMapW = boost::get(boost::edge_weight, curRes.m_Graph);

        boost::put(propMapW, edgeId.first, vec.Length());
      }
    }

    Penumbra openings;

    for(auto& component : resComps)
    {
      for(auto& face : component.m_Faces)
      {
        Vector2f faceCenter = face.m_Box.GetCenter();
        Vector2f faceDim = face.m_Box.GetSize();

        if(face.m_Edges.empty())
        {
          Vector2f corners[] =
          {
            face.m_Box.m_Data[0],
            face.m_Box.m_Data[0] + faceDim.X() * Vector2f::UNIT_X,
            face.m_Box.m_Data[0] + faceDim.Y() * Vector2f::UNIT_Y,
            face.m_Box.m_Data[0] + faceDim.X() * Vector2f::UNIT_X + faceDim.Y() * Vector2f::UNIT_Y
          };

          face.m_Walls.push_back(Segmentf({corners[2], corners[0]}));
          face.m_Walls.push_back(Segmentf({corners[0], corners[1]}));
          face.m_Walls.push_back(Segmentf({corners[1], corners[3]}));
          face.m_Walls.push_back(Segmentf({corners[3], corners[2]}));
          for (auto& val : face.m_HasSide) { val = true; }
          continue;
        }

        Vector2f ccwDirs[4] =
        {
           Vector2f::UNIT_Y,
          -Vector2f::UNIT_X,
          -Vector2f::UNIT_Y,
           Vector2f::UNIT_X
        };

        for (auto& val : face.m_HasSide) { val = false; }
        for(uint32_t dirIdx = 0; dirIdx < 4; ++dirIdx)
        {
          float sign = dirIdx >= 2 ? -1.0: 1.0;
          int32_t axis = dirIdx % 2;

          Vector2f dir;
          dir.m_Data[axis] = sign;

          openings.Start(faceCenter, 0.0, dir, faceDim.SquaredLength());

          openings.AddSegment(Segmentf({faceCenter - dir, faceCenter + faceDim.m_Data[axis] * dir - faceDim.m_Data[1 - axis] * MathTools::Perp(dir)}), 0.0);
          openings.AddSegment(Segmentf({faceCenter - dir, faceCenter + faceDim.m_Data[axis] * dir + faceDim.m_Data[1 - axis] * MathTools::Perp(dir)}), 0.0);

          for(auto segIdx : face.m_Edges)
          {
            Segmentf const& seg = component.m_FaceEdges[segIdx].segment;
            Vector2f segDir = seg.m_Ext2 - seg.m_Ext1;
            int32_t segAxis = segDir.X() == 0 ? 0 : 1;
            float segSign = Mathf::Sign(seg.m_Ext1.m_Data[axis] - faceCenter.m_Data[axis]);

            if(segAxis == axis && segSign == sign)
            {
              openings.AddSegment(component.m_FaceEdges[segIdx].segment, 0.0);
            }
          }

          Vector<Segmentf> ranges = openings.GetAllOpenings();
          if (!ranges.empty())
          {
            face.m_HasSide[dirIdx] = true;
          }
          for(auto const& range : ranges)
          {
            Segmentf localRanges;
            localRanges.m_Ext1 = MathTools::GetLocal(dir, range.m_Ext1);
            localRanges.m_Ext2 = MathTools::GetLocal(dir, range.m_Ext2);

            face.m_Walls.push_back(Segmentf());
            Segmentf& newWall = face.m_Walls.back();
            newWall.m_Ext1.X() = faceDim.m_Data[axis] * 0.5;
            newWall.m_Ext1.Y() = newWall.m_Ext1.X() * localRanges.m_Ext1.Y() / localRanges.m_Ext1.X();
            newWall.m_Ext1 = faceCenter + newWall.m_Ext1.X() * dir + newWall.m_Ext1.Y() * MathTools::GetPerp(dir);

            newWall.m_Ext2.X() = faceDim.m_Data[axis] * 0.5;
            newWall.m_Ext2.Y() = newWall.m_Ext2.X() * localRanges.m_Ext2.Y() / localRanges.m_Ext2.X();
            newWall.m_Ext2 = faceCenter + newWall.m_Ext2.X() * dir + newWall.m_Ext2.Y() * MathTools::GetPerp(dir);

            if ((newWall.m_Ext2 - newWall.m_Ext1).Dot(ccwDirs[dirIdx]) < 0)
            {
              std::swap(newWall.m_Ext2, newWall.m_Ext1);
            }
          }
        }
      }
    }

    NavMesh result;
    result.m_Components = std::move(resComps);

    return result;
  }

  boost::optional<NavMesh::FoundFace> NavMesh::FindFace(Vector2f const& iPos) const
  {
    Vector<BoxIndexEntry> results;
    AABB2Df queryBox;
    queryBox.m_Data[0] = iPos - Vector2f::ONE;
    queryBox.m_Data[1] = iPos + Vector2f::ONE;

    for (uint32_t i = 0; i< m_Components.size(); ++i)
    {
      auto const& component = m_Components[i];
      component.m_FacesIdx.query(
        boost::geometry::index::intersects(queryBox),
        std::back_inserter(results));

      for (auto const& res : results)
      {
        if (res.first.Contains(iPos, 0))
        {
          FoundFace face = { i, res.second };
          return face;
        }
      }
    }

    return boost::none;
  }

  namespace
  {
    struct found_goal
    {
      NavMesh::Graph::vertex_descriptor outEdge;
    };

    class astar_goal_visitor : public boost::default_astar_visitor
    {
    public:
      inline astar_goal_visitor(NavMesh::EdgeMap const& iEdges, uint32_t iGoalFace)
        : m_Edges(iEdges)
        , m_Goal(iGoalFace)
      {
      }

      void examine_vertex(NavMesh::Graph::vertex_descriptor u, NavMesh::Graph const& g)
      {
        eXl_ASSERT(u < m_Edges.size());
        auto const& desc = m_Edges[u];
        //if(desc != m_Edges.end())
        {
          if(desc.face1 == m_Goal
          || desc.face2 == m_Goal)
          {
            found_goal goal = {u};
            throw goal;
          }
        }
      }
    private:
      NavMesh::EdgeMap const& m_Edges;
      uint32_t m_Goal;
    };

    class distance_heuristic : public boost::astar_heuristic<NavMesh::Graph, float>
    {
    public:
      typedef boost::graph_traits<NavMesh::Graph>::vertex_descriptor Vertex;
      distance_heuristic(NavMesh::EdgeMap const& iEdges, Vector2f iGoalPt)
        : m_Edges(iEdges)
        , m_GoalPt(iGoalPt)
      {}

      float operator()(Vertex u)
      {
        eXl_ASSERT(u < m_Edges.size());
        auto const& desc = m_Edges[u];
        //if(desc != m_Edges.end())
        {
          Vector2f edgeCenter = (desc.segment.m_Ext1 + desc.segment.m_Ext2) * 0.5;

          return (edgeCenter - m_GoalPt).Length();
        }

        return FLT_MAX;
      }
    private:
      NavMesh::EdgeMap const& m_Edges;
      Vector2f m_GoalPt;
    };
  }

  boost::optional<uint32_t> NavMesh::Edge::CommonFace(Edge const& iOther) const
  {
    if(face1 == iOther.face1
    || face1 == iOther.face2)
    {
      return face1;
    }
    else
    if(face2 == iOther.face1
    || face2 == iOther.face2)
    {
      return face2;
    }
    return boost::none;
  }

  Vector2f GetDirToFace(AABB2Df const& iOrigFace, AABB2Df const& iDestFace)
  {
    int touchId = iOrigFace.Touch(iDestFace);
    switch(touchId)
    {
    case 1:
      return Vector2f::UNIT_X * -1.0;
      break;
    case 2:
      return Vector2f::UNIT_X;
      break;
    case 3:
      return Vector2f::UNIT_Y * -1.0;
      break;
    case 4:
      return Vector2f::UNIT_Y;
      break;
    default:
      //eXl_ASSERT_MSG(false, "Neighbouring faces not touching");
      return Vector2f::ZERO;
      break;
    }
  }

  boost::optional<NavMesh::Path> NavMesh::FindPath(Vector2f const& iStart, Vector2f const& iEnd) const
  {
    boost::optional<FoundFace> faceStartId = FindFace(iStart);
    boost::optional<FoundFace> faceEndId = FindFace(iEnd);

    if(faceStartId && faceEndId)
    {
      if(*faceStartId == *faceEndId)
      {
        return NavMesh::Path();
      }

      if (faceStartId->m_Component != faceEndId->m_Component)
      {
        return boost::none;
      }

      Component const& component = m_Components[faceStartId->m_Component];

      Face const& faceStart = component.m_Faces[faceStartId->m_Face];
      Face const& faceEnd = component.m_Faces[faceEndId->m_Face];

      eXl_ASSERT_REPAIR_RET(!faceStart.m_Edges.empty() && !faceStart.m_Edges.empty(), boost::none)
      {
        IndexMap<Graph> outIndex(component.m_Graph);

        auto startVtx = faceStart.m_Edges.front();

        Vector<Graph::vertex_descriptor> p(boost::num_vertices(component.m_Graph), startVtx);
        try
        {
          boost::astar_search_tree
          (component.m_Graph, startVtx,
            distance_heuristic(component.m_FaceEdges, iEnd),
            boost::predecessor_map(boost::make_iterator_property_map(p.begin(), outIndex)).
            visitor(astar_goal_visitor(component.m_FaceEdges, faceEndId->m_Face)));
        }
        catch(found_goal& goal)
        {
          uint32_t curFaceId = faceEndId->m_Face;
          Vector2f goalPt(iEnd.X(), iEnd.Y());
          Path res;
          res.m_Component = faceStartId->m_Component;
          for(auto v = goal.outEdge;; v = p[v])
          {
            res.m_Edges.push_back(v);

            eXl_ASSERT_REPAIR_RET(v < component.m_FaceEdges.size(), boost::none);

            auto const& edgeDesc = component.m_FaceEdges[v];
            //eXl_ASSERT_REPAIR_RET(edgeDesc != component.m_FaceEdges.end(), boost::none)
            {
              uint32_t prevFaceId = edgeDesc.face1 == curFaceId ? edgeDesc.face2 : edgeDesc.face1;

              Segmentf const& seg = edgeDesc.segment;
              auto midPt = (seg.m_Ext1 + seg.m_Ext2) * 0.5;

              auto dirToGoal = goalPt - midPt;

              if(dirToGoal.Length() < Mathf::ZERO_TOLERANCE)
              {
                dirToGoal = (seg.m_Ext1 - seg.m_Ext2);
                dirToGoal.Normalize();
                res.m_EdgeDirs.push_back(dirToGoal);
              }
              else
              {
                Face const& curFace = component.m_Faces[curFaceId];
                Face const& prevFace = component.m_Faces[prevFaceId];

                auto perpDir = GetDirToFace(prevFace.m_Box, curFace.m_Box);

                res.m_EdgeDirs.push_back(perpDir);
              }

              goalPt = midPt;
              curFaceId = prevFaceId;
            }

            if(p[v] == v)
              break;
          }

          if(res.m_Edges.size() >= 2)
          {
            auto const& edge1 = component.m_FaceEdges[*(--res.m_Edges.end())];
            auto const& edge2 = component.m_FaceEdges[*(--(--res.m_Edges.end()))];

            auto commonFaceIdx = edge1.CommonFace(edge2);

            eXl_ASSERT_REPAIR_RET(!(!commonFaceIdx), boost::none);
            {
              if(*commonFaceIdx == faceStartId->m_Face)
              {
                res.m_Edges.pop_back();
                res.m_EdgeDirs.pop_back();
              }
            }
          }

          if(res.m_Edges.size() >= 2)
          {
            auto const& edge1 = component.m_FaceEdges[*(res.m_Edges.begin())];
            auto const& edge2 = component.m_FaceEdges[*(++res.m_Edges.begin())];

            auto commonFaceIdx = edge1.CommonFace(edge2);

            eXl_ASSERT_REPAIR_RET(!(!commonFaceIdx), boost::none);
            {
              if(*commonFaceIdx == faceEndId->m_Face)
              {
                res.m_Edges.erase(res.m_Edges.begin());
                res.m_EdgeDirs.erase(res.m_EdgeDirs.begin());
              }
            }
          }

          return res;
        }
      }
    }

    return boost::none;
  }
}