/*
 * image_volume.cc
 *
 *  Copyright (C) 2013 Diamond Light Source
 *
 *  Author: James Parkhurst
 *
 *  This code is distributed under the BSD license, a copy of which is
 *  included in the root directory of this package.
 */
#include <boost/python.hpp>
#include <boost/python/def.hpp>
#include <scitbx/array_family/flex_types.h>
#include <dials/array_family/reflection_table.h>
#include <dials/model/data/image_volume.h>
#include <dials/model/data/mask_code.h>
#include <dials/config.h>

namespace dials { namespace model { namespace boost_python {

  using namespace boost::python;
  using af::BackgroundIncludesBadPixels;
  using af::ForegroundIncludesBadPixels;

  template <typename FloatType>
  void MultiPanelImageVolume_update_reflection_info(
    MultiPanelImageVolume<FloatType> image_volume,
    af::reflection_table reflections) {
    // Check the input
    DIALS_ASSERT(reflections.contains("bbox"));
    DIALS_ASSERT(reflections.contains("panel"));

    // Get some reflection table columns
    af::const_ref<int6> bbox = reflections["bbox"];
    af::const_ref<std::size_t> panel = reflections["panel"];

    // Get the flags array
    af::ref<std::size_t> flags = reflections["flags"];

    // Set some information about number of pixels
    af::ref<std::size_t> num_valid = reflections["num_pixels.valid"];
    af::ref<std::size_t> num_bg = reflections["num_pixels.background"];
    // af::ref<std::size_t> num_bg_used = reflections["num_pixels.background_used"];
    af::ref<std::size_t> num_fg = reflections["num_pixels.foreground"];
    af::ref<std::size_t> num_ol = reflections["num_pixels.overlapped"];

    // Set some background information
    af::ref<double> bg_mean = reflections["background.mean"];
    af::ref<double> bg_disp = reflections["background.dispersion"];

    // The mask codes
    int mask_code1 = Valid;
    int mask_code2 = Valid | Background;
    int mask_code3 = Valid | Background | BackgroundUsed;
    int mask_code4 = Valid | Foreground;
    int mask_code5 = Overlapped;

    // Loop through each reflection
    for (std::size_t i = 0; i < panel.size(); ++i) {
      // Get the image volume
      ImageVolume<FloatType> v = image_volume.get(panel[i]);
      DIALS_ASSERT(v.is_consistent());

      // Trim the bounding box
      int6 b = v.trim_bbox(bbox[i]);

      // Get the data arrays
      af::versa<FloatType, af::c_grid<3> > data = v.extract_data(b);
      af::versa<FloatType, af::c_grid<3> > bgrd = v.extract_background(b);
      af::versa<int, af::c_grid<3> > mask = v.extract_mask(b, i);

      // Compute numbers of pixels
      std::size_t num1 = 0;
      std::size_t num2 = 0;
      std::size_t num3 = 0;
      std::size_t num4 = 0;
      std::size_t num5 = 0;
      std::size_t num6 = 0;
      std::size_t num7 = 0;
      for (std::size_t j = 0; j < mask.size(); ++j) {
        if ((mask[j] & mask_code1) == mask_code1) num1++;
        if ((mask[j] & mask_code2) == mask_code2) num2++;
        if ((mask[j] & mask_code3) == mask_code3) num3++;
        if ((mask[j] & mask_code4) == mask_code4) num4++;
        if ((mask[j] & mask_code5) == mask_code5) num5++;
        if ((mask[j] & Background) && !(mask[j] & Valid)) num6++;
        if ((mask[j] & Foreground) && !(mask[j] & Valid)) num7++;
      }
      num_valid[i] = num1;
      num_bg[i] = num2;
      // num_bg_used[i] = num3;
      num_fg[i] = num4;
      num_ol[i] = num5;

      // Set some flags
      if (num6 > 0) {
        flags[i] |= BackgroundIncludesBadPixels;
      } else {
        flags[i] &= ~BackgroundIncludesBadPixels;
      }
      if (num7 > 0) {
        flags[i] |= ForegroundIncludesBadPixels;
      } else {
        flags[i] &= ~ForegroundIncludesBadPixels;
      }

      // Compute the mean (modelled) background and dispersion in background
      double fg_sum = 0.0;
      double fg_cnt = 0.0;
      double bg_sum = 0.0;
      double bg_cnt = 0.0;
      double bg_sum_sq = 0.0;
      for (std::size_t j = 0; j < mask.size(); ++j) {
        if (mask[j] & Foreground) {
          fg_sum += bgrd[j];
          fg_cnt += 1;
        }
        if ((mask[j] | mask_code2) == mask_code2) {
          bg_sum += data[j];
          bg_sum_sq += data[j] * data[j];
          bg_cnt += 1;
        }
      }
      if (fg_cnt > 0) {
        bg_mean[i] = fg_sum / fg_cnt;
      } else {
        bg_mean[i] = 0.0;
      }
      if (bg_cnt > 0 && bg_sum > 0) {
        bg_disp[i] = bg_sum_sq / bg_sum - bg_sum / bg_cnt;
      } else {
        bg_disp[i] = 0.0;
      }
    }
  }

  template <typename FloatType>
  void image_volume_wrapper(const char *name) {
    typedef ImageVolume<FloatType> Class;

    class_<Class>(name, no_init)
      .def(init<int, int, std::size_t, std::size_t>())
      .def("frame0", &Class::frame0)
      .def("frame1", &Class::frame1)
      .def("accessor", &Class::accessor)
      .def("data", &Class::data)
      .def("background", &Class::background)
      .def("mask", &Class::mask)
      .def("set_image", &Class::template set_image<int>)
      .def("set_image", &Class::template set_image<double>)
      .def("is_consistent", &Class::is_consistent)
      .def("extract_data", &Class::extract_data)
      .def("extract_background", &Class::extract_background)
      .def("extract_mask", &Class::extract_mask);
  }

  template <typename FloatType>
  void multi_panel_image_volume_wrapper(const char *name) {
    typedef MultiPanelImageVolume<FloatType> Class;

    class_<Class>(name)
      .def("frame0", &Class::frame0)
      .def("frame1", &Class::frame1)
      .def("add", &Class::add)
      .def("get", &Class::get)
      .def("set_image", &Class::template set_image<int>)
      .def("set_image", &Class::template set_image<double>)
      .def("update_reflection_info",
           &MultiPanelImageVolume_update_reflection_info<FloatType>)
      .def("__len__", &Class::size);
  }

  void export_image_volume() {
    typedef ImageVolume<>::float_type FloatType;

    image_volume_wrapper<FloatType>("ImageVolume");
    multi_panel_image_volume_wrapper<FloatType>("MultiPanelImageVolume");
  }

}}}  // namespace dials::model::boost_python


//==================================================================================================
/*!
  @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_RROR_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_RROR_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

  /*!
    @ingroup group-bitwise
    This function object returns the first entry rotated right or left
    by the absolute value of the second entry,
    according to its sign.

    @par Header <boost/simd/function/rror.hpp>

    @par Note
    This function asserts if the integral parameter is out of range

    @see  ror, rrol, ror

    @par Example:

      @snippet rror.cpp rror

    @par Possible output:

      @snippet rror.txt rror

  **/
  Value rror(Value const& x, IntegerValue const& n);
} }
#endif

#include <boost/simd/function/scalar/rror.hpp>
#include <boost/simd/function/simd/rror.hpp>

#endif


#pragma once

#include <boost/preprocessor.hpp>

//! 何も展開しない
#define NOTHING
//! 引数を任意個受け取り、何も展開しない
#define NOTHING_ARG(...)

//! 2の乗数シーケンス
#define SEQ_SHIFT	(1)(2)(4)(8)(16)(32)(64)(128)(256)
//! 右シフト
#define PP_RSHIFT(n,s)	BOOST_PP_DIV(n, BOOST_PP_SEQ_ELEM(s, SEQ_SHIFT))
//! 左シフト
#define PP_LSHIFT(n,s)	BOOST_PP_MUL(n, BOOST_PP_SEQ_ELEM(s, SEQ_SHIFT))
//! 指定の位置のビットを取り出す
#define PP_GETBIT(num,at)	BOOST_PP_MOD(PP_RSHIFT(num,at), 2)

// --------------- 指定クラスのイテレータ関数をそのまま呼ぶ ---------------
// bit0		[_][c][_][_][c][_]...
// bit0|1	[_][c][c][_][c][c]...
// bit2		[_][_][_][r][r][r]...
// bit3		[b][b][b][b][b][b][e][e][e][e][e][e]...
// 但しbit0&1が1の時はskip

#define ITR_ADAPTOR_LOCAL_2(src, itrname, func, c_post) \
				itrname func() c_post { return src.func(); }
#define ITR_ADAPTOR_LOCAL_1(src, c_pre, ci_pre, c_post, r_pre, ri_pre, dir) \
				ITR_ADAPTOR_LOCAL_2(src, \
				BOOST_PP_CAT(ci_pre, BOOST_PP_CAT(ri_pre, iterator)), \
				BOOST_PP_CAT(c_pre, BOOST_PP_CAT(r_pre, dir)), \
				c_post)
#define ITR_ADAPTOR_LOCAL_0(src, b0,b1,b2,b3) \
				ITR_ADAPTOR_LOCAL_1(src, \
				BOOST_PP_IF(b0, c, NOTHING), \
				BOOST_PP_IF(BOOST_PP_BITOR(b1,b0), const_, NOTHING), \
				BOOST_PP_IF(BOOST_PP_BITOR(b1,b0), const, NOTHING), \
				BOOST_PP_IF(b2, r, NOTHING), \
				BOOST_PP_IF(b2, reverse_, NOTHING), \
				BOOST_PP_IF(b3, end, begin))
//! b0とb1が共に1ならば何も展開しない
#define ITR_CLIP_INVALID(src,b0,b1,b2,b3)	BOOST_PP_IF(BOOST_PP_BITAND(b0, b1), NOTHING_ARG, ITR_ADAPTOR_LOCAL_0)(src,b0,b1,b2,b3)
//! nの下位4ビットを展開してCLIP_INVALIDに渡す
#define ITERATOR_ADAPTOR(z,n,src)			ITR_CLIP_INVALID(src, PP_GETBIT(n,0), PP_GETBIT(n,1), PP_GETBIT(n,2), PP_GETBIT(n,3))
//! 大文字アルファベットシーケンス
#define SEQ_ALPHABET (A)(B)(C)(D)(E)(F)(G)(H)(I)(J)(K)(L)(M)(N)(O)(P)(Q)(R)(S)(T)(U)(V)(W)(X)(Y)(Z)
//! tupleのsequenceを定義
#define MAKE_SEQ(size, rel) MAKE_SEQ_D(size, rel)
#define MAKE_SEQ_D(size, rel) \
	BOOST_PP_CAT( \
		MAKE_SEQ_A_ ## size rel, \
		0X0 \
	)() \
	/**/

// size 2
#define MAKE_SEQ_A_2(x, y) ((x, y)) MAKE_SEQ_B_2
#define MAKE_SEQ_B_2(x, y) ((x, y)) MAKE_SEQ_A_2
// size 3
#define MAKE_SEQ_A_3(x, y, z) ((x, y, z)) MAKE_SEQ_B_3
#define MAKE_SEQ_B_3(x, y, z) ((x, y, z)) MAKE_SEQ_A_3
// size 4
#define MAKE_SEQ_A_4(x, y, z, w) ((x, y, z, w)) MAKE_SEQ_B_4
#define MAKE_SEQ_B_4(x, y, z, w) ((x, y, z, w)) MAKE_SEQ_A_4

#define MAKE_SEQ_A_20X0()
#define MAKE_SEQ_B_20X0()
#define MAKE_SEQ_A_30X0()
#define MAKE_SEQ_B_30X0()
#define MAKE_SEQ_A_40X0()
#define MAKE_SEQ_B_40X0()

// Move CtorでMoveするオブジェクトをSequenceによって指定
#define Move_CtorInner(z, obj, elem)	(elem(std::move(obj.elem)))
#define Move_Ctor(seq, obj)				BOOST_PP_SEQ_ENUM(BOOST_PP_SEQ_FOR_EACH(Move_CtorInner, obj, seq))
#define Move_AssignInner(z, obj, elem)	elem = std::move(obj.elem);
#define Move_Assign(seq, obj)			BOOST_PP_SEQ_FOR_EACH(Move_AssignInner, obj, seq)


//   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 CLOTHO_BATCH_CROSSOVER_TASK_POPULATION_SPACE_ROW_HPP_
#define CLOTHO_BATCH_CROSSOVER_TASK_POPULATION_SPACE_ROW_HPP_

#include "clotho/data_spaces/population_space/population_space_row.hpp"
#include <boost/random/bernoulli_distribution.hpp>
#include "clotho/data_spaces/crossover/position_classifier.hpp"
#include "clotho/data_spaces/generators/position_distribution_helper.hpp"
#include "clotho/data_spaces/generators/crossover_event_distribution_helper.hpp"

#include "clotho/data_spaces/crossover/block_crossover_method.hpp"

#include "clotho/data_spaces/task/task.hpp"

namespace clotho {
namespace genetics {

template < class RNG, class MatePairType, class BlockType, class WeightType, class AlleleSpaceType >
class batch_crossover_task< RNG, MatePairType, population_space_row< BlockType, WeightType > , AlleleSpaceType > : public task {
public:
    typedef batch_crossover_task< RNG, MatePairType, population_space_row< BlockType, WeightType >, AlleleSpaceType > self_type;

    typedef population_space_row< BlockType, WeightType >       space_type;
    typedef AlleleSpaceType                                     allele_type;
    typedef RNG                                                 random_engine_type;

    typedef typename space_type::block_type                     block_type;

    typedef typename space_type::individual_pointer             individual_pointer;
    typedef typename space_type::genome_pointer                 genome_pointer;

    typedef MatePairType                                        mate_pair_type;
    typedef typename mate_pair_type::iterator                   iterator;
    typedef typename mate_pair_type::const_iterator             const_iterator;

    typedef PositionClassifier< typename allele_type::position_vector > classifier_type;
    typedef typename classifier_type::event_type                        event_type;

    typedef block_crossover_method< classifier_type, BlockType >     method_type;
    typedef typename method_type::bit_helper_type                    bit_helper_type;

    typedef typename position_distribution_helper< typename allele_type::position_type >::type  position_distribution_type;
    typedef typename crossover_event_distribution_helper< double >::type                    event_distribution_type;

    batch_crossover_task( random_engine_type * rng, space_type * parents, space_type * offspring, allele_type * alleles, unsigned int off_idx, const_iterator first, const_iterator last, double recomb_rate, double seq_bias ) :
        m_rng( rng )
        , m_parent_pop( parents )
        , m_offspring_pop( offspring )
        , m_alleles( alleles )
        , m_parents( first, last )
        , m_offspring_index( off_idx )
        , m_recomb_rate( recomb_rate )
        , m_seq_bias( seq_bias )
    {}

    batch_crossover_task( const self_type & other ) :
        m_rng( other.m_rng )
        , m_parent_pop( other.m_parent_pop )
        , m_offspring_pop( other.m_offspring_pop )
        , m_alleles( other.m_alleles )
        , m_parents( other.m_parents )
        , m_offspring_index( other.m_offspring_index )
        , m_recomb_rate( other.m_recomb_rate )
        , m_seq_bias( other.m_seq_bias )
    { }

    void operator()() {

        event_distribution_type     event_dist( m_recomb_rate);
        boost::random::bernoulli_distribution< double > bias_dist( m_seq_bias);

        const_iterator mate_it = m_parents.begin(), mate_end = m_parents.end();

        size_t i = m_offspring_index * 2;

        while( mate_it != mate_end ) {

            generate_parental_crossover( 2 * mate_it->first, i++, event_dist(*m_rng), bias_dist(*m_rng) );
            generate_parental_crossover( 2 * mate_it->second, i, event_dist(*m_rng), bias_dist(*m_rng) );

            ++mate_it;
        }
    }

    virtual ~batch_crossover_task() {}

protected:
    void generate_parental_crossover( unsigned int parent_idx, unsigned int offspring_idx, unsigned int N, bool _swap ) {
        genome_pointer p0_start = m_parent_pop->begin_genome( parent_idx );
        genome_pointer p0_end = m_parent_pop->end_genome( parent_idx++ );

        genome_pointer p1_start = m_parent_pop->begin_genome( parent_idx );
        genome_pointer p1_end = m_parent_pop->end_genome( parent_idx );

        genome_pointer c = m_offspring_pop->begin_genome( offspring_idx );
        genome_pointer c_end = m_offspring_pop->end_genome( offspring_idx );

        event_type evts;
        fill_events( evts, N );
        classifier_type cfier0( &m_alleles->getPositions(), evts );

        run_crossover_task2( cfier0, p0_start, p0_end, p1_start, p1_end, c, c_end, _swap );
    }

    inline void fill_events( event_type & evt, unsigned int N ) {
        while( N-- ) {
            evt.push_back( m_pos_dist( *m_rng ) );
        }
    }

    event_type make_events( unsigned int N ) {
        event_type res;

        while( N-- ) {
            res.push_back( m_pos_dist( *m_rng ) );
        }

        return res;
    }

    void run_crossover_task2( const classifier_type & cls, genome_pointer p0_start, genome_pointer p0_end, genome_pointer p1_start, genome_pointer p1_end, genome_pointer offspring, genome_pointer offspring_end, bool should_swap_strands ) {
        if( cls.event_count() == 0 ) {
            genome_pointer first = p0_start, last = p0_end;
            if( should_swap_strands ) {
                first = p1_start;
                last = p1_end;
            }

            while( first != last ) {
                *offspring++ = *first++;
            }

            while( offspring != offspring_end ) {
                *offspring++ = bit_helper_type::ALL_UNSET;
            }
        } else {
            method_type met(cls);

            if( should_swap_strands ) {
                met( p1_start, p1_end, p0_start, p0_end, offspring, offspring_end);
            } else {
                met( p0_start, p0_end, p1_start, p1_end, offspring, offspring_end);
            }
        }
    }

/*
    void run_crossover_task( const classifier_type & cls, genome_pointer p0_start, genome_pointer p0_end, genome_pointer p1_start, genome_pointer p1_end, genome_pointer offspring, genome_pointer offspring_end, bool should_swap_strands ) {
        if( cls.event_count() == 0 ) {
            genome_pointer first = p0_start, last = p0_end;
            if( should_swap_strands ) {
                first = p1_start;
                last = p1_end;
            }

            while( first != last ) {
                *offspring++ = *first++;
            }

            while( offspring != offspring_end ) {
                *offspring++ = bit_helper_type::ALL_UNSET;
            }
        } else if( should_swap_strands ) {
            crossover_task( cls, p1_start, p1_end, p0_start, p0_end, offspring, offspring_end);
        } else {
            crossover_task( cls, p0_start, p0_end, p1_start, p1_end, offspring, offspring_end);
        }
    }

    void crossover_task( const classifier_type & cls, genome_pointer p0_start, genome_pointer p0_end, genome_pointer p1_start, genome_pointer p1_end,  genome_pointer offspring, genome_pointer offspring_end ) {
//        std::cerr << "Parent length: " << (p0_end - p0_start) << " x " << (p1_end - p1_start) << std::endl;

//        std::cerr << "Parent Population Bounds: " << PARENT_ALLELE_COUNT << " alleles x " << PARENT_STEP << " genomes" << std::endl;
//        std::cerr << "Offspring Population Bounds: " << OFFSPRING_ALLELE_COUNT << " alleles x " << OFFSPRING_STEP << " genomes" << std::endl;
        method_type xover( cls );

        unsigned int i = 0;
        while( true ) {
            if( p0_start == p0_end ) {
                while( p1_start != p1_end ) {
                    const block_type t = bit_helper_type::ALL_UNSET;
                    const block_type b = *p1_start++;
                    *offspring++ = xover.crossover( t, b, i );

                    i += bit_helper_type::BITS_PER_BLOCK;
                }
                break;
            } else if( p1_start == p1_end ) {
                while( p0_start != p0_end ) {
                    const block_type t = *p0_start++;
                    const block_type b = bit_helper_type::ALL_UNSET;
                    *offspring++ = xover.crossover( t, b, i );
                    i += bit_helper_type::BITS_PER_BLOCK;
                }
                break;
            }

            const block_type t = *p0_start++;
            const block_type b = *p1_start++;

            *offspring++ = xover.crossover( t, b, i );
            i += bit_helper_type::BITS_PER_BLOCK;
        }

        while( offspring != offspring_end ) {
            *offspring++ = bit_helper_type::ALL_UNSET;
        }
    }
*/
//    void crossover_task2( const classifier_type & cls, genome_pointer p0_start, genome_pointer p0_end, genome_pointer p1_start, genome_pointer p1_end,  genome_pointer offspring, genome_pointer offspring_end ) {
////        std::cerr << "Parent length: " << (p0_end - p0_start) << " x " << (p1_end - p1_start) << std::endl;
//
////        std::cerr << "Parent Population Bounds: " << PARENT_ALLELE_COUNT << " alleles x " << PARENT_STEP << " genomes" << std::endl;
////        std::cerr << "Offspring Population Bounds: " << OFFSPRING_ALLELE_COUNT << " alleles x " << OFFSPRING_STEP << " genomes" << std::endl;
//        method_type xover( cls );
//
//        block_type  buffer[ bit_helper_type::BLOCKS_PER_CACHE_LINE ];
//        unsigned int het_buffer[ bit_helper_type::BITS_PER_CACHE_LINE ];
//
//        assert( (p0_end - p0_start) % bit_helper_type::BLOCKS_PER_CACHE_LINE == 0);
//        assert( (p1_end - p1_start) % bit_helper_type::BLOCKS_PER_CACHE_LINE == 0);
//        assert( (p0_end - p0_start) == (p1_end - p1_start) );
//
//        const unsigned int CACHE_LINES = (p0_end - p0_start) / bit_helper_type::BLOCKS_PER_CACHE_LINE;
//
//        for( unsigned int i = 0; i < CACHE_LINES; ++i ) {
//            unsigned int nHets = 0;
//            for( unsigned int j = 0; j < bit_helper_type::BLOCKS_PER_CACHE_LINE; ++j ) {
//                block_type t = *p0_start++;
//                block_type b = *p1_start++;
//
//                buffer[ j ] = a & b;
//                block_type hets = a ^ b;
//
//                unsigned int idx = j * bit_helper_type::BITS_PER_BLOCK;
//                while( hets ) {
//                    idx += bit_walker_type::next_and_shift(hets );
//                    het_buffer[ nHets++ ] = idx;
//                }
//            }
//
//            unsigned int offset = bit_helper_type::BITS_PER_CACHE_LINE * i;
//            for( unsigned int j = 0; j < nHets; ++j ) {
//
//            }
//
//            for( unsigned int j = 0; j < bit_helper_type::BLOCKS_PER_CACHE_LINE; ++j ) {
//                *offspring++ = buffer[ j ];
//            }
//        }
//
//        unsigned int i = 0;
//        while( true ) {
//            if( p0_start == p0_end ) {
//                while( p1_start != p1_end ) {
//                    const block_type t = bit_helper_type::ALL_UNSET;
//                    const block_type b = *p1_start++;
//                    *offspring++ = xover.crossover( t, b, i );
//
//                    i += bit_helper_type::BITS_PER_BLOCK;
//                }
//                break;
//            } else if( p1_start == p1_end ) {
//                while( p0_start != p0_end ) {
//                    const block_type t = *p0_start++;
//                    const block_type b = bit_helper_type::ALL_UNSET;
//                    *offspring++ = xover.crossover( t, b, i );
//                    i += bit_helper_type::BITS_PER_BLOCK;
//                }
//                break;
//            }
//
//            const block_type t = *p0_start++;
//            const block_type b = *p1_start++;
//
//            *offspring++ = xover.crossover( t, b, i );
//            i += bit_helper_type::BITS_PER_BLOCK;
//        }
//
//        while( offspring != offspring_end ) {
//            *offspring++ = bit_helper_type::ALL_UNSET;
//        }
//    }
    random_engine_type  * m_rng;
    space_type * m_parent_pop, * m_offspring_pop;
    allele_type         * m_alleles;

    mate_pair_type m_parents;

    unsigned int m_offspring_index;

    double m_recomb_rate, m_seq_bias;

    position_distribution_type  m_pos_dist;
};

}   // namespace genetics
}   // namesapce clotho

#endif  // CLOTHO_BATCH_CROSSOVER_TASK_POPULATION_SPACE_ROW_HPP_


//----------------------------------*-C++-*----------------------------------//
/**
 *  @file  Mesh.hh
 *  @brief Mesh class definition.
 *  @note  Copyright (C) 2013 Jeremy Roberts
 */
//---------------------------------------------------------------------------//

#ifndef detran_geometry_MESH_HH_
#define detran_geometry_MESH_HH_

#include "geometry/geometry_export.hh"
#include "utilities/Definitions.hh"
#include "utilities/DBC.hh"
#include "utilities/Point.hh"
#include "utilities/SP.hh"
#include "utilities/Warning.hh"
#include <cmath>
#include <map>
#ifdef DETRAN_ENABLE_BOOST
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/string.hpp>
#endif

namespace detran_geometry
{

//---------------------------------------------------------------------------//
/**
 *  @class Mesh
 *  @brief Abstract Cartesian mesh class.
 *
 *  Note, the constructors are protected to forbid direct instantiation of
 *  the Mesh class.  Rather, use the dimension-specific subclasses.  We
 *  could use a pure virtual destructor as an alternative.
 */
//---------------------------------------------------------------------------//
class GEOMETRY_EXPORT Mesh
{

public:

  //-------------------------------------------------------------------------//
  // ENUMERATIONS
  //-------------------------------------------------------------------------//

  enum SIDES
  {
    WEST,
    EAST,
    SOUTH,
    NORTH,
    BOTTOM,
    TOP,
    END_SIDES
  };

  enum FACE2D
  {
    VERT, HORZ
  };

  enum FACE3D
  {
    YZ, XZ, XY
  };

  //-------------------------------------------------------------------------//
  // TYPEDEFS
  //-------------------------------------------------------------------------//

  typedef detran_utilities::SP<Mesh>        SP_mesh;
  typedef detran_utilities::vec_int         vec_int;
  typedef detran_utilities::vec_dbl         vec_dbl;
  typedef std::map<std::string, vec_int>    mesh_map_type;
  typedef detran_utilities::size_t          size_t;

  // Note, these constructors are protected to hide them from the
  // user.  These are to be called by inherited classes.  I keep
  // the constructors at the top for convention.
protected:

  /**
   *  @brief Constructor.
   *
   *  @param    dim         Spatial dimension
   *  @param    xfm         Fine meshes per coarse mesh in x dimension.
   *  @param    yfm         Fine meshes per coarse mesh in y dimension.
   *  @param    zfm         Fine meshes per coarse mesh in z dimension.
   *  @param    xcme        Coarse mesh edges x dimension.
   *  @param    ycme        Coarse mesh edges y dimension.
   *  @param    zcme        Coarse mesh edges z dimension.
   */
  Mesh(size_t dim,
       vec_int xfm,  vec_int yfm,  vec_int zfm,
       vec_dbl xcme, vec_dbl ycme, vec_dbl zcme,
       vec_int mat_map);

  /**
   *  @brief Constructor.
   *
   *  @param    dim         Spatial dimension
   *  @param    xfme        Fine mesh edges x dimension.
   *  @param    yfme        Fine mesh edges y dimension.
   *  @param    zfme        Fine mesh edges z dimension.
   */
  Mesh(size_t dim,
       vec_dbl xfme, vec_dbl yfme, vec_dbl zfme,
       vec_int mat_map);

  Mesh(size_t dim) : d_dimension(dim) {}

public:

  /// Virtual destructor
  virtual ~Mesh(){}

  //------------------------------------------------------------------------//
  // Setters
  //------------------------------------------------------------------------//

  /**
   * @brief  Add map of coarse mesh integer properties.
   *
   * This is an easy way to set mesh properties for meshes based on
   * simple coarse mesh regions.
   *
   * @param  map_key   String description of map.
   * @param  mesh_map  Logically multi-dimensional map as 1-d vector.
   */
  void add_coarse_mesh_map(std::string map_key, vec_int mesh_map);

  /**
   * @brief  Add map of fine mesh integer properties.
   *
   * This adds properties for fine meshes directly, and so is meanty for
   * use with higher level mesh construction, e.g. pin cells, where
   * assignment is not possible by simple coarse mesh bounds.
   *
   * @note If the key exists, this function overwrites the map.
   *
   * @param  map_key   String description of map.
   * @param  mesh_map  Logically multi-dimensional map as 1-d vector.
   */
  void add_mesh_map(std::string map_key, vec_int mesh_map);

  //------------------------------------------------------------------------//
  // Getters
  //------------------------------------------------------------------------//

  /// Return total number of cells.
  size_t number_cells() const;

  /// Return number of cells in specified dimension.
  size_t number_cells(size_t dim) const;
  /// Return number of cells along x axis
  size_t number_cells_x() const;
  /// Return number of cells along y axis
  size_t number_cells_y() const;
  /// Return number of cells along z axis
  size_t number_cells_z() const;

  /// Get cell width in a specified dimension
  double width(size_t dim, size_t ijk) const;
  /// Get cell width along x axis
  double dx(size_t i) const;
  /// Get cell width along y axis
  double dy(size_t j) const;
  /// Get cell width along z axis
  double dz(size_t k) const;

  /// Get vector of widths along x axis
  const vec_dbl& dx() const;
  /// Get vector of widths along y axis
  const vec_dbl& dy() const;
  /// Get vector of widths along z axis
  const vec_dbl& dz() const;

  /// Get the cell volume
  double volume(size_t cell) const;

  /// Get domain width along x axis
  double total_width_x() const;
  /// Get domain width along y axis
  double total_width_y() const;
  /// Get domain width along z axis
  double total_width_z() const;

  /// Get the mesh dimension
  size_t dimension() const;

  /**
   * @brief   Returns the cardinal index for i, j, and k
   * @param   i  Index along x axis.
   * @param   j  Index along y axis.
   * @param   k  Index along z axis.
   * @return     Cardinal index.
   */
  size_t index(size_t i, size_t j = 0, size_t k = 0);

  /**
   *  @brief   Returns the x index given cardinal index
   *  @param   cell  Cardinal index.
   *  @return        Index along x axis.
   */
  size_t cell_to_i(size_t cell) const;

  /**
   *  @brief   Returns the y index given cardinal index
   *  @param   cell  Cardinal index.
   *  @return        Index along y axis.
   */
  size_t cell_to_j(size_t cell) const;

  /**
   *  @brief   Returns the z index given cardinal index
   *  @param   cell  Cardinal index.
   *  @return        Index along z axis.
   */
  size_t cell_to_k(size_t cell) const;

  /// Find the cell containing a point
  int find_cell(detran_utilities::Point p);

  /// Check if fine mesh map exists.
  bool mesh_map_exists(std::string map_key);

  /**
   *  @brief  Get map of fine mesh integer properties.
   *
   *  This adds properties for fine meshes directly, and so is meant for
   *  use with higher level mesh construction, e.g. pin cells, where
   *  assignment is not possible by simple coarse mesh bounds.
   *
   *  @param   m  Logically multi-dimensional map as 1-d vector.
   */
  const vec_int& mesh_map(std::string map_key);

  /// Return a const reference to the full map (useful for IO)
  const mesh_map_type& get_mesh_map() const;

  /// Display some key features
  void display() const;

protected:

  /// Mesh spatial dimension
  size_t d_dimension;
  /// x fine meshes in each x coarse mesh
  vec_int d_xfm;
  /// y fine meshes in each y coarse mesh
  vec_int d_yfm;
  /// z fine meshes in each y coarse mesh
  vec_int d_zfm;
  /// x coarse mesh edges
  vec_dbl d_xcme;
  /// y coarse mesh edges
  vec_dbl d_ycme;
  /// z coarse mesh edges
  vec_dbl d_zcme;
  /// x widths
  vec_dbl d_dx;
  /// y widths
  vec_dbl d_dy;
  /// z widths
  vec_dbl d_dz;
  /// total x width
  double d_total_width_x;
  /// total y width
  double d_total_width_y;
  /// total z width
  double d_total_width_z;
  /// Total number of cells
  size_t d_number_cells;
  /// Number of cells in x direction
  size_t d_number_cells_x;
  /// Number of cells in y direction
  size_t d_number_cells_y;
  /// Number of cells in y direction
  size_t d_number_cells_z;

  /**
   *  Map container containing a key describing a mesh property and a fine
   *  mesh map defining the property in each cell.  These properties
   *  include materials, coarse mesh regions (pins, assembly, fuel,
   *  moderator, etc.), and anything else the user wants to edit.
   */
  mesh_map_type d_mesh_map;

private:

  /// Common construction tasks.
  void setup();

#ifdef DETRAN_ENABLE_BOOST

  /// Default constructor.  This is required for serialization and
  /// is of no use otherwise.
  Mesh(){}

  friend class boost::serialization::access;
  template <class Archive>
  void serialize(Archive & ar, const unsigned int version)
  {
    ar & d_dimension;
    ar & d_xfm;
    ar & d_yfm;
    ar & d_zfm;
    ar & d_xcme;
    ar & d_ycme;
    ar & d_zcme;
    ar & d_dx;
    ar & d_dy;
    ar & d_dz;
    ar & d_total_width_x;
    ar & d_total_width_y;
    ar & d_total_width_z;
    ar & d_number_cells;
    ar & d_number_cells_x;
    ar & d_number_cells_y;
    ar & d_number_cells_z;
    ar & d_mesh_map;
  }

#endif

};

GEOMETRY_TEMPLATE_EXPORT(detran_utilities::SP<Mesh>)

} // end namespace detran_geometry

//---------------------------------------------------------------------------//
// INLINE FUNCTIONS
//---------------------------------------------------------------------------//

#include "Mesh.i.hh"

#endif /* detran_geometry_MESH_HH_ */

//---------------------------------------------------------------------------//
//              end of Mesh.hh
//---------------------------------------------------------------------------//


#include <Eigen/Dense>

namespace rosette {

using Eigen::Ref;

template <typename T>
using VecX = Eigen::Matrix<T, Eigen::Dynamic, 1>;
template <typename T>
using RefVecX = Eigen::Ref<VecX<T>>;
template <typename T>
using CRefVecX = Eigen::Ref<VecX<T> const>;

template <typename T>
using MatX = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
template <typename T>
using RefMatX = Eigen::Ref<MatX<T>>;
template <typename T>
using CRefMatX = Eigen::Ref<MatX<T> const>;

template <typename T>
using MatX4 = Eigen::Matrix<T, Eigen::Dynamic, 4, Eigen::RowMajor>;
template <typename T>
using RefMatX4 = Eigen::Ref<MatX4<T>>;
template <typename T>
using CRefMatX4 = Eigen::Ref<MatX4<T> const>;

using VecXi1 = VecX<int8_t>;
using VecXi2 = VecX<int16_t>;
using VecXi4 = VecX<int32_t>;
using VecXi8 = VecX<int64_t>;
// using VecXi = VecX<int>;
using VecXu1 = VecX<uint8_t>;
using VecXu2 = VecX<uint16_t>;
using VecXu4 = VecX<uint32_t>;
using VecXu8 = VecX<uint64_t>;
// using VecXu = VecX<uint>;
using VecXf = VecX<float>;
using VecXd = VecX<double>;

using RefVecXi1 = RefVecX<int8_t>;
using RefVecXi2 = RefVecX<int16_t>;
using RefVecXi4 = RefVecX<int32_t>;
using RefVecXi8 = RefVecX<int64_t>;
// using RefVecXi = RefVecX<int>;
using RefVecXu1 = RefVecX<uint8_t>;
using RefVecXu2 = RefVecX<uint16_t>;
using RefVecXu4 = RefVecX<uint32_t>;
using RefVecXu8 = RefVecX<uint64_t>;
// using RefVecXu = RefVecX<uint>;
using RefVecXf = RefVecX<float>;
using RefVecXd = RefVecX<double>;

using CRefVecXi1 = CRefVecX<int8_t>;
using CRefVecXi2 = CRefVecX<int16_t>;
using CRefVecXi4 = CRefVecX<int32_t>;
using CRefVecXi8 = CRefVecX<int64_t>;
// using CRefVecXi = CRefVecX<int>;
using CRefVecXu1 = CRefVecX<uint8_t>;
using CRefVecXu2 = CRefVecX<uint16_t>;
using CRefVecXu4 = CRefVecX<uint32_t>;
using CRefVecXu8 = CRefVecX<uint64_t>;
// using CRefVecXu = CRefVecX<uint>;
using CRefVecXf = CRefVecX<float>;
using CRefVecXd = CRefVecX<double>;

using MatXi1 = MatX<int8_t>;
using MatXi2 = MatX<int16_t>;
using MatXi4 = MatX<int32_t>;
using MatXi8 = MatX<int64_t>;
// using MatXi = MatX<int>;
using MatXu1 = MatX<uint8_t>;
using MatXu2 = MatX<uint16_t>;
using MatXu4 = MatX<uint32_t>;
using MatXu8 = MatX<uint64_t>;
// using MatXu = MatX<uint>;
using MatXf = MatX<float>;
using MatXd = MatX<double>;

using RefMatXi1 = RefMatX<int8_t>;
using RefMatXi2 = RefMatX<int16_t>;
using RefMatXi4 = RefMatX<int32_t>;
using RefMatXi8 = RefMatX<int64_t>;
// using RefMatXi = RefMatX<int>;
using RefMatXu1 = RefMatX<uint8_t>;
using RefMatXu2 = RefMatX<uint16_t>;
using RefMatXu4 = RefMatX<uint32_t>;
using RefMatXu8 = RefMatX<uint64_t>;
// using RefMatXu = RefMatX<uint>;
using RefMatXf = RefMatX<float>;
using RefMatXd = RefMatX<double>;

using CRefMatXi1 = CRefMatX<int8_t>;
using CRefMatXi2 = CRefMatX<int16_t>;
using CRefMatXi4 = CRefMatX<int32_t>;
using CRefMatXi8 = CRefMatX<int64_t>;
// using CRefMatXi = CRefMatX<int>;
using CRefMatXu1 = CRefMatX<uint8_t>;
using CRefMatXu2 = CRefMatX<uint16_t>;
using CRefMatXu4 = CRefMatX<uint32_t>;
using CRefMatXu8 = CRefMatX<uint64_t>;
// using CRefMatXu = CRefMatX<uint>;
using CRefMatXf = CRefMatX<float>;
using CRefMatXd = CRefMatX<double>;

using MatX4i1 = MatX4<int8_t>;
using MatX4i2 = MatX4<int16_t>;
using MatX4i4 = MatX4<int32_t>;
using MatX4i8 = MatX4<int64_t>;
// using MatX4i = MatX4<int>;
using MatX4u1 = MatX4<uint8_t>;
using MatX4u2 = MatX4<uint16_t>;
using MatX4u4 = MatX4<uint32_t>;
using MatX4u8 = MatX4<uint64_t>;
// using MatX4u = MatX4<uint>;
using MatX4f = MatX4<float>;
using MatX4d = MatX4<double>;

using RefMatX4i1 = RefMatX4<int8_t>;
using RefMatX4i2 = RefMatX4<int16_t>;
using RefMatX4i4 = RefMatX4<int32_t>;
using RefMatX4i8 = RefMatX4<int64_t>;
// using RefMatX4i = RefMatX4<int>;
using RefMatX4u1 = RefMatX4<uint8_t>;
using RefMatX4u2 = RefMatX4<uint16_t>;
using RefMatX4u4 = RefMatX4<uint32_t>;
using RefMatX4u8 = RefMatX4<uint64_t>;
// using RefMatX4u = RefMatX4<uint>;
using RefMatX4f = RefMatX4<float>;
using RefMatX4d = RefMatX4<double>;

using CRefMatX4i1 = CRefMatX4<int8_t>;
using CRefMatX4i2 = CRefMatX4<int16_t>;
using CRefMatX4i4 = CRefMatX4<int32_t>;
using CRefMatX4i8 = CRefMatX4<int64_t>;
// using CRefMatX4i = CRefMatX4<int>;
using CRefMatX4u1 = CRefMatX4<uint8_t>;
using CRefMatX4u2 = CRefMatX4<uint16_t>;
using CRefMatX4u4 = CRefMatX4<uint32_t>;
using CRefMatX4u8 = CRefMatX4<uint64_t>;
// using CRefMatX4u = CRefMatX4<uint>;
using CRefMatX4f = CRefMatX4<float>;
using CRefMatX4d = CRefMatX4<double>;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SATURATE_AT_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_IEEE_FUNCTIONS_SIMD_COMMON_SATURATE_AT_HPP_INCLUDED
#include <boost/simd/toolbox/ieee/functions/saturate_at.hpp>
#include <boost/dispatch/functor/meta/make_functor.hpp>
#include <boost/simd/include/functions/simd/unary_minus.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/min.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_at_<Tag>
                                    , tag::cpu_
                                    , (A0)(Tag)(X)
                                    , ((simd_<arithmetic_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typename dispatch::make_functor<Tag, A0>::type callee;
      const A0 z = callee( dispatch::meta::as_<A0>() );
      return select(gt(a0, z), z, select(lt(a0, -z), -z, a0));
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION ( boost::simd::tag::saturate_at_<Tag>
                                    , tag::cpu_
                                    , (A0)(Tag)(X)
                                    , ((simd_<unsigned_<A0>,X>))
                                   )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL(1)
    {
      typename dispatch::make_functor<Tag, A0>::type callee;
      return boost::simd::min(a0, callee( dispatch::meta::as_<A0>() ));
    }
  };
} } }

#endif


#ifndef __ITERATOR_HH__
#define __ITERATOR_HH__

#include <cstdlib>
#include <Eigen/Dense>
#include "parse.hh"

namespace hashpca
{
  typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>
          MatrixXd;

  template<typename S,
           typename T>
  struct Times
    {
      S left;
      T right;

      Times (S _left, T _right) : left (_left), right (_right) { }
    };

  template<typename T>
  struct Transposed
    {
      T t;

      Transposed (T _t) : t (_t) { }

      template<typename Right>
      Times<Transposed, Right&>
      operator* (Right& r)
        {
          return Times<Transposed, Right&> (*this, r);
        }
    };

  template<typename T>
  Times<double, Transposed<T> >
  operator* (double             x,
             Transposed<T>      t)
    {
      return Times<double, Transposed<T> > (x, t);
    }

  class LinearIterator
    {
      private:
        uint64_t hashsize;

      public:
        LinearIterator (uint64_t _hashsize) : hashsize (_hashsize) { }

        struct Deferred
          {
            uint64_t hashsize;
            const veedubparse::GeneralExample& ex;

            Deferred (uint64_t                           _hashsize,
                      const veedubparse::GeneralExample& _ex)
              : hashsize (_hashsize),
                ex (_ex)
              {
              }

            template<typename F>
            void
            foreach (F f) const
              {
                for (unsigned int n = 0; n < 256; ++n)
                  {
                    const veedubparse::Feature* fptr = &ex.f[n][0];

                    for (unsigned int j = 0; j < ex.f[n].size (); ++j, ++fptr)
                      {
                        veedubparse::Feature x (fptr->id % hashsize, fptr->value);

                        f (x);
                      }
                  }
              }

            Transposed<Deferred>
            transpose ()
              {
                return Transposed<Deferred> (*this);
              }
          };

        Deferred
        operator() (const veedubparse::GeneralExample& ex)
          {
            return Deferred (hashsize, ex);
          }
    };

  Times<double, LinearIterator::Deferred>
  operator* (double                     x,
             LinearIterator::Deferred   d)
    {
      return Times<double, LinearIterator::Deferred> (x, d);
    }

  class BiLinearIterator
    {
      private:
        uint64_t        hashsize;
        unsigned char   a;
        unsigned char   b;

        static const uint64_t quadratic_constant = 27942141;

      public:
        BiLinearIterator (uint64_t      _hashsize,
                          unsigned char _a,
                          unsigned char _b)
          : hashsize (_hashsize),
            a (_a),
            b (_b)
          {
          }

        struct Deferred
          {
            uint64_t hashsize;
            unsigned char a;
            unsigned char b;
            const veedubparse::GeneralExample& ex;

            Deferred (uint64_t                           _hashsize,
                      unsigned char                      _a,
                      unsigned char                      _b,
                      const veedubparse::GeneralExample& _ex)
              : hashsize (_hashsize),
                a (_a),
                b (_b),
                ex (_ex)
              {
              }

            template<typename F>
            void
            foreach (F f) const
              {
                const veedubparse::Feature* iptr = &ex.f[a][0];

                for (unsigned int i = 0; i < ex.f[a].size (); ++i, ++iptr)
                  {
                    const veedubparse::Feature* jptr = &ex.f[b][0];

                    for (unsigned int j = 0; j < ex.f[b].size (); ++j, ++jptr)
                      {
                        veedubparse::Feature
                          x ((quadratic_constant * iptr->id + jptr->id) % hashsize,
                             iptr->value * jptr->value);

                        f (x);
                      }
                  }
              }

            Transposed<Deferred>
            transpose ()
              {
                return Transposed<Deferred> (*this);
              }
          };

        Deferred
        operator() (const veedubparse::GeneralExample& ex)
          {
            return Deferred (hashsize, a, b, ex);
          }
    };

  Times<double, BiLinearIterator::Deferred>
  operator* (double                     x,
             BiLinearIterator::Deferred d)
    {
      return Times<double, BiLinearIterator::Deferred> (x, d);
    }

  struct SugaryVectorXd : public Eigen::VectorXd
    {
      SugaryVectorXd (unsigned int rows) : Eigen::VectorXd (rows) { };

      template<typename Iterator>
      SugaryVectorXd&
      operator= (Times<Transposed<Iterator>, const hashpca::MatrixXd&> op)
        {
          setZero (size ());

          op.left.t.foreach ([ &op, this ] (const veedubparse::Feature& f) {
            *this += f.value * op.right.row (f.id);
          });

          return *this;
        }
    };

  template<typename T>
  Times<Times<double, T>, SugaryVectorXd&>
  operator* (Times<double, T>  left,
             SugaryVectorXd&   right)
    {
      return Times<Times<double, T>, SugaryVectorXd&> (left, right);
    }

  template<typename Iterator>
  hashpca::MatrixXd&
  operator+= (hashpca::MatrixXd&                                x,
              Times<Times<double, Iterator>, SugaryVectorXd&>   op)
    {
      op.left.right.foreach ([&] (const veedubparse::Feature& f) {
        x.row (f.id) += op.left.left * f.value * op.right;
      });

      return x;
    }

  template<typename Iterator>
  Eigen::VectorXd&
  operator+= (Eigen::VectorXd&                          x,
              Times<double, Transposed<Iterator> >      op)
    {
      op.right.t.foreach ([&] (const veedubparse::Feature& f) {
        x (f.id) += op.left * f.value;
      });

      return x;
    }
}

#endif // __ITERATOR_HH__


/*M///////////////////////////////////////////////////////////////////////////////////////
//
//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//
// Copyright (C) 2014 Takuya MINAGAWA.
// Third party copyrights are property of their respective owners.
//
// 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.
//
//M*/

#include "DataAugmentation.h"
#include <opencv2/highgui/highgui.hpp>
#include <boost/filesystem/path.hpp>
#include <iostream>
#include "RandomRotation.h"
#include "Util.h"

cv::Rect RandomDeformRect(const cv::Rect& input_rect, double x_slide_sigma, double y_slide_sigma,
	double aspect_range, cv::RNG& rng)
{
	double x_mv_r = rng.gaussian(x_slide_sigma);
	double y_mv_r = rng.gaussian(y_slide_sigma);
	double aspect_change = rng.gaussian(aspect_range);

	cv::Rect dst_rect;
	double deform = aspect_change / (2.0 + aspect_change);
	dst_rect.width = input_rect.width * (1.0 + deform);
	dst_rect.height = input_rect.height * (1.0 - deform);
	dst_rect.x = input_rect.x + (input_rect.width - dst_rect.width) / 2;
	dst_rect.y = input_rect.y + (input_rect.height - dst_rect.height) / 2;

	dst_rect.x += x_mv_r * dst_rect.width;
	dst_rect.y += y_mv_r * dst_rect.height;

	return dst_rect;
}

cv::Mat ImageTransform(const cv::Mat& img, const cv::Rect& area,
	double yaw_sigma, double pitch_sigma, double roll_sigma,
	double blur_max_sigma, double noise_max_sigma, double x_slide_sigma, double y_slide_sigma,
	double aspect_range, double hflip_ratio, double vflip_ratio, cv::RNG& rng)
{
	assert(img.type() == CV_8UC1 || img.type() == CV_8UC3);

	// Deform Rect Randomly
	cv::Rect rect = (area.width <= 0 || area.height <= 0) ? cv::Rect(0, 0, img.cols, img.rows) :
		RandomDeformRect(area, x_slide_sigma, y_slide_sigma, aspect_range, rng);

	rect = util::TruncateRect(rect, img.size());

	// Random Rotation
	cv::Mat dst;
	RandomRotateImage(img, dst, yaw_sigma, pitch_sigma, roll_sigma, rect, rng);

	// Random Noise
	double noise_sigma = rng.uniform(0.0, noise_max_sigma);
	if (noise_sigma > 0){
		cv::Mat gauss_noise(dst.size(), CV_32FC(dst.channels()));
		cv::randn(gauss_noise, 0.0, noise_sigma);
		int num = dst.cols * dst.rows * dst.channels();
		unsigned char* dst_ptr = dst.data;
		float* noise_ptr = (float*)gauss_noise.data;
		for (int i = 0; i < num; i++){
			int val = *dst_ptr + *noise_ptr;
			*dst_ptr = (val > 255) ? 255 : (val < 0) ? 0 : val;
			dst_ptr++, noise_ptr++;
		}
	}

	// Random Blur
	cv::Mat dst2;
	double blur_sigma = rng.uniform(0.0, blur_max_sigma);
	int size = blur_sigma * 2.5 + 0.5;
	size += (1 - size % 2);
	if (blur_sigma > 0 && size >= 3){
		cv::Size ksize(size, size);
		cv::GaussianBlur(dst, dst2, ksize, blur_sigma);
	}
	else{
		dst2 = dst;
	}

	// Rondom Flip (horizontal)
	cv::Mat dst3;
	double flip_prob = rng.uniform(0.0, 1.0);
	if (hflip_ratio > flip_prob) {
		cv::flip(dst2, dst3, 1);
	}
	else {
		dst3 = dst2;
	}

	// Rondom Flip (vertical)
	cv::Mat dst4;
	flip_prob = rng.uniform(0.0, 1.0);
	if (vflip_ratio > flip_prob) {
		cv::flip(dst3, dst4, 0);
	}
	else {
		dst4 = dst3;
	}
	return dst4;
}

void DataAugmentation(const std::vector<std::string>& img_files, const std::vector<std::vector<cv::Rect>>& areas,
	const std::string& output_folder, const std::string& output_file,
	int num_generate, double yaw_range, double pitch_range, double roll_range,
	double blur_sigma, double noise_sigma, double x_slide, double y_slide, double aspect_range,
	double hflip_ratio, double vflip_ratio)
{
	assert(areas.empty() || areas.size() == img_files.size());

	using namespace boost::filesystem;

	cv::RNG rng;
	int num_img = img_files.size();
	for (int i = 0; i < num_img; i++){
		std::cout << "Load " << img_files[i] << std::endl;
		cv::Mat img = cv::imread(img_files[i]);
		if (img.empty())
			continue;

		cv::Rect pos(0, 0, img.cols, img.rows);
		std::vector <cv::Rect> trans_areas;
		if (areas.empty()){
			trans_areas.push_back(pos);
		}
		else{
			trans_areas = areas[i];
		}

		for (int j = 0; j < trans_areas.size(); j++){
			std::stringstream filestr;
			filestr << "img" << i << "_" << j;

			for (int k = 0; k < num_generate; k++){
				cv::Mat tran_img = ImageTransform(img, trans_areas[j], yaw_range, pitch_range, roll_range,
					blur_sigma, noise_sigma, x_slide, y_slide, aspect_range, hflip_ratio, vflip_ratio, rng);
				std::stringstream filestr2;
				filestr2 << filestr.str() << "_" << k << ".png";
				path dst_file = path(output_folder) / path(filestr2.str());
				std::string save_img_name = dst_file.string();

				std::cout << "Save image " << save_img_name << "...";
				if (cv::imwrite(dst_file.string(), tran_img)){
					std::vector<cv::Rect> pos;
					pos.push_back(cv::Rect(0,0,tran_img.cols, tran_img.rows));
					util::AddAnnotationLine(output_file, dst_file.string(), pos, " ");
					std::cout << "succeed";
				}
				else{
					std::cout << "fail";
				}
				std::cout << std::endl;
			}
		}
	}
}


#include <limits>
#include "Mods.h"
#include "MatchSummary.h"

#include <boost/dynamic_bitset.hpp>
#include <windows.h>

#include "detours.h"

#ifdef _DEBUG
namespace Logger
{
    extern void error(const char *format, ...);
}
#endif

#pragma warning (disable : 4996)

namespace MatchSummary
{

    struct AwardData
    {
        int tierlimit[3];
        int weight;
    };

    // Get rid of the max macro from some Windows header file
    #ifdef max
    #undef max
    #endif
    #define no_tiers {1, std::numeric_limits<int>::max(), std::numeric_limits<int>::max()}

    const int defaultWeight = 1;

    static const std::map<int,AwardData> cStatValueMap = {
        { CONST_STAT_AWD_CREDITS_EARNED              , {{1, 10000, 20000}, 95} },
        { CONST_STAT_AWD_CREDITS_SPENT               , {{1, 5000, 15000}, 89} },
        { CONST_STAT_AWD_DESTRUCTION_DEPLOYABLE      , {{1, 3, 10}, 84} },
        { CONST_STAT_AWD_DESTRUCTION_VEHICLE         , {{1, 2, 4}, 88} },
        { CONST_STAT_AWD_DISTANCE_HEADSHOT           , {{1, 300, 600}, 87} },
        { CONST_STAT_AWD_DISTANCE_KILL               , {{1, 400, 1000}, 93} },
        { CONST_STAT_AWD_DISTANCE_SKIED              , {{1, 40000, 100000}, 90} },
        { CONST_STAT_AWD_KILLS                       , {{1, 6, 15}, 86} },
        { CONST_STAT_AWD_KILLS_DEPLOYABLE            , {{1, 3, 8}, 92} },
        { CONST_STAT_AWD_KILLS_MIDAIR                , {{1, 3, 8}, 94} },
        { CONST_STAT_AWD_KILLS_VEHICLE               , {{1, 4, 10}, 81} },
        { CONST_STAT_AWD_REPAIRS                     , {{1, 8, 40}, 96} },
        { CONST_STAT_AWD_SPEED_FLAGCAP               , {{-99999, -60, -25}, -100} },
        { CONST_STAT_AWD_SPEED_FLAGGRAB              , {{1, 100, 200}, 97} },
        { CONST_STAT_AWD_SPEED_SKIED                 , {{1, 200, 260}, 95} },
        { CONST_STAT_AWD_FLAG_RETURNS                , {{1, 3, 7}, 85} },
        { CONST_STAT_AWD_DEATHS                      , {{1, 6, 15}, 1} },
        { CONST_STAT_ACO_KS_FIVE                     , {no_tiers, CONST_STAT_ACOW_KS_FIVE               } },
        { CONST_STAT_ACO_KS_TEN                      , {no_tiers, CONST_STAT_ACOW_KS_TEN                } },
        { CONST_STAT_ACO_KS_FIFTEEN                  , {no_tiers, CONST_STAT_ACOW_KS_FIFTEEN            } },
        { CONST_STAT_ACO_KS_TWENTY                   , {no_tiers, CONST_STAT_ACOW_KS_TWENTY             } },
        { CONST_STAT_ACO_KS_TWENTYFIVE               , {no_tiers, CONST_STAT_ACOW_KS_TWENTYFIVE         } },
        { CONST_STAT_ACO_KS_FIVE_SNIPING             , {no_tiers, CONST_STAT_ACOW_KS_FIVE_SNIPING       } },
        { CONST_STAT_ACO_KS_TEN_SNIPING              , {no_tiers, CONST_STAT_ACOW_KS_TEN_SNIPING        } },
        { CONST_STAT_ACO_KS_FIFTEEN_SNIPING          , {no_tiers, CONST_STAT_ACOW_KS_FIFTEEN_SNIPING    } },
        { CONST_STAT_ACO_KS_FIVE_EXPLOSIVE           , {no_tiers, CONST_STAT_ACOW_KS_FIVE_EXPLOSIVE     } },
        { CONST_STAT_ACO_KS_TEN_EXPLOSIVE            , {no_tiers, CONST_STAT_ACOW_KS_TEN_EXPLOSIVE      } },
        { CONST_STAT_ACO_KS_FIFTEEN_EXPLOSIVE        , {no_tiers, CONST_STAT_ACOW_KS_FIFTEEN_EXPLOSIVE  } },
        { CONST_STAT_ACO_KS_FIVE_SPINFUSOR           , {no_tiers, CONST_STAT_ACOW_KS_FIVE_SPINFUSOR     } },
        { CONST_STAT_ACO_KS_TEN_SPINFUSOR            , {no_tiers, CONST_STAT_ACOW_KS_TEN_SPINFUSOR      } },
        { CONST_STAT_ACO_KS_FIFTEEN_SPINFUSOR        , {no_tiers, CONST_STAT_ACOW_KS_FIFTEEN_SPINFUSOR  } },
        { CONST_STAT_ACO_MK_DOUBLE                   , {no_tiers, CONST_STAT_ACOW_MK_DOUBLE             } },
        { CONST_STAT_ACO_MK_TRIPLE                   , {no_tiers, CONST_STAT_ACOW_MK_TRIPLE             } },
        { CONST_STAT_ACO_MK_QUATRA                   , {no_tiers, CONST_STAT_ACOW_MK_QUATRA             } },
        { CONST_STAT_ACO_MK_ULTRA                    , {no_tiers, CONST_STAT_ACOW_MK_ULTRA              } },
        { CONST_STAT_ACO_MK_TEAM                     , {no_tiers, CONST_STAT_ACOW_MK_TEAM               } },
        { CONST_STAT_ACO_NOJOY                       , {no_tiers, CONST_STAT_ACOW_NOJOY                 } },
        { CONST_STAT_ACO_REVENGE                     , {no_tiers, CONST_STAT_ACOW_REVENGE               } },
        { CONST_STAT_ACO_AFTERMATH                   , {no_tiers, CONST_STAT_ACOW_AFTERMATH             } },
        { CONST_STAT_ACO_FIRSTBLOOD                  , {no_tiers, CONST_STAT_ACOW_FIRSTBLOOD            } },
        { CONST_STAT_ACO_BLUEPLATESPECIAL            , {no_tiers, CONST_STAT_ACOW_BLUEPLATESPECIAL      } },
        { CONST_STAT_ACO_STICKYKILL                  , {no_tiers, CONST_STAT_ACOW_STICKYKILL            } },
        { CONST_STAT_ACO_HEADSHOT                    , {no_tiers, CONST_STAT_ACOW_HEADSHOT              } },
        { CONST_STAT_ACO_ARTILLERYSTRIKE             , {no_tiers, CONST_STAT_ACOW_ARTILLERYSTRIKE       } },
        { CONST_STAT_ACO_MELEE                       , {no_tiers, CONST_STAT_ACOW_MELEE                 } },
        { CONST_STAT_ACO_ROADKILL                    , {no_tiers, CONST_STAT_ACOW_ROADKILL              } },
        { CONST_STAT_ACO_FLAG_CAPTURE                , {no_tiers, CONST_STAT_ACOW_FLAG_CAPTURE          } },
        { CONST_STAT_ACO_FLAG_GRAB                   , {no_tiers, CONST_STAT_ACOW_FLAG_GRAB             } },
        { CONST_STAT_ACO_BK_GEN                      , {no_tiers, CONST_STAT_ACOW_BK_GEN                } },
        { CONST_STAT_ACO_RABBITKILL                  , {no_tiers, CONST_STAT_ACOW_RABBITKILL            } },
        { CONST_STAT_ACO_KILLASRABBIT                , {no_tiers, CONST_STAT_ACOW_KILLASRABBIT          } },
        { CONST_STAT_ACO_FINALBLOW                   , {no_tiers, CONST_STAT_ACOW_FINALBLOW             } },
        { CONST_STAT_ACO_REPAIR                      , {no_tiers, CONST_STAT_ACOW_REPAIR                } },
        { CONST_STAT_ACO_BK_TURRET                   , {no_tiers, CONST_STAT_ACOW_BK_TURRET             } },
        { CONST_STAT_ACO_ASSIST                      , {no_tiers, CONST_STAT_ACOW_ASSIST                } },
        { CONST_STAT_ACO_FLAG_RETURN                 , {no_tiers, CONST_STAT_ACOW_FLAG_RETURN           } },
        { CONST_STAT_ACO_BK_RADAR                    , {no_tiers, CONST_STAT_ACOW_BK_RADAR              } },
        { CONST_STAT_ACO_FLAG_ASSIST                 , {no_tiers, CONST_STAT_ACOW_FLAG_ASSIST           } },
        { CONST_STAT_ACO_AIRMAIL                     , {no_tiers, CONST_STAT_ACOW_AIRMAIL               } },
        { CONST_STAT_ACO_GAME_COMPLETE               , {no_tiers, CONST_STAT_ACOW_GAME_COMPLETE         } },
        { CONST_STAT_ACO_GAME_WINNER                 , {no_tiers, CONST_STAT_ACOW_GAME_WINNER           } },
        { CONST_STAT_ACO_FLAG_GRABDM                 , {no_tiers, CONST_STAT_ACOW_FLAG_GRABDM           } },
        { CONST_STAT_ACO_FLAG_HOLDER                 , {no_tiers, CONST_STAT_ACOW_FLAG_HOLDER           } },
        { CONST_STAT_ACO_FLAG_KILLER                 , {no_tiers, CONST_STAT_ACOW_FLAG_KILLER           } },
        { CONST_STAT_ACO_FLAG_GRABFAST               , {no_tiers, CONST_STAT_ACOW_FLAG_GRABFAST         } },
        { CONST_STAT_ACO_DEFENSE_GEN                 , {no_tiers, CONST_STAT_ACOW_DEFENSE_GEN           } },
        { CONST_STAT_ACO_DEFENSE_FLAG                , {no_tiers, CONST_STAT_ACOW_DEFENSE_FLAG          } },
        { CONST_STAT_ACO_VD_BIKE                     , {no_tiers, CONST_STAT_ACOW_VD_BIKE               } },
        { CONST_STAT_ACO_VD_TANK                     , {no_tiers, CONST_STAT_ACOW_VD_TANK               } },
        { CONST_STAT_ACO_VD_SHRIKE                   , {no_tiers, CONST_STAT_ACOW_VD_SHRIKE             } },
        { CONST_STAT_ACO_FLAG_GRABE                  , {no_tiers, CONST_STAT_ACOW_FLAG_GRABE            } },
        { CONST_STAT_ACO_FLAG_GRABLLAMA              , {no_tiers, CONST_STAT_ACOW_FLAG_GRABLLAMA        } },
        { CONST_STAT_ACO_ASSIST_VEHICLE              , {no_tiers, CONST_STAT_ACOW_ASSIST_VEHICLE        } },
        { CONST_STAT_ACO_FLAG_GRABULTRA              , {no_tiers, CONST_STAT_ACOW_FLAG_GRABULTRA        } },
        { CONST_STAT_ACO_BENCHEM                     , {no_tiers, 2                                     } },
        { CONST_STAT_ACO_DOUBLEDOWN                  , {no_tiers, 20                                    } },
        { CONST_STAT_ACO_LASTMANSTANDING             , {no_tiers, 2                                     } },
        { CONST_STAT_ACO_MIRACLE                     , {no_tiers, 100                                   } },
        { CONST_STAT_ACO_NOTAMONGEQUALS              , {no_tiers, 40                                    } },
        { CONST_STAT_ACO_ONEMANARMY                  , {no_tiers, 60                                    } },
        { CONST_STAT_ACO_TRIBALHONOR                 , {no_tiers, 2                                     } },
        { CONST_STAT_ACO_UNITEDWESTAND               , {no_tiers, 10                                    } },
        { CONST_STAT_ACO_HOLDTHELINE                 , {no_tiers, 1                                     } },
        { CONST_STAT_ACO_CAPTUREANDHOLD              , {no_tiers, 2                                     } },
        { CONST_STAT_ACO_BASEASSIST                  , {no_tiers, CONST_STAT_ACOW_BASEASSIST            } },
        { CONST_STAT_ACO_TURRETASSIST                , {no_tiers, CONST_STAT_ACOW_TURRETASSIST          } },
        { CONST_STAT_ACO_HOTAIR                      , {no_tiers, CONST_STAT_ACOW_HOTAIR                } },
    };

    StatsCollector sStatsCollector;

    typedef void(__thiscall* EndGameFunction)(void*, int);
    typedef int(__thiscall* Level6Function)(void*, int, int);
    typedef void(__cdecl* Level8Function)(void*, int, void*, int, int);
    typedef int (__stdcall *SendtoFunction)(SOCKET s, const char* buf, int len, int flags, const sockaddr* to, int tolen);

    struct ClientData
    {
        ClientData() :
            seqnr(0),
            previousWriteOffset(0),
            fragmentInserted(false),
            insertionOffset(0),
            insertionCount(0),
            fragmentSent(false),
            maxNumberOfBits(0)
        {
            OutputDebugString(L"tamods: New client data created\n");
        }

        unsigned int seqnr;
        int previousWriteOffset;
        bool fragmentInserted;
        int insertionOffset;
        int insertionCount;
        bool fragmentSent;
        int maxNumberOfBits;
    };

    static EndGameFunction realEndGameFunction = 0;
    static Level6Function realLevel6Function = 0;
    static Level8Function realLevel8Function = 0;
    static SendtoFunction realSendtoFunction = 0;

    static int gameEnded = 0;
    static bool firstLevel8SinceLevel6 = false;
    static std::map<void*, ClientData> clientMap;
    static std::map<char *, unsigned int> bufferToPlayerId;

    void __fastcall fakeEndGameFunction(void* pThis, void*, int a)
    {
        OutputDebugString(L"tamods: fakeEndGameFunction\n");
        gameEnded++;
        realEndGameFunction(pThis, a);
    }

    int __fastcall fakeLevel6Function(void* pThis, void*, int a, int b)
    {
        //OutputDebugString(L"tamods: fakeLevel6Function\n");
        firstLevel8SinceLevel6 = true;
        unsigned int playerId = *((unsigned int*)pThis + 0xb8 / sizeof(unsigned int));
        char *pSendBuffer = *((char **)pThis + 0x2e8 / sizeof(char *));

#ifdef _DEBUG
        auto it = bufferToPlayerId.find(pSendBuffer);
        if(it != bufferToPlayerId.end() && it->second != playerId)
        {
            Logger::error("First player %08X and then player 0x%X was using buffer %08X", it->second, playerId, pSendBuffer);
        }
#endif
        bufferToPlayerId[pSendBuffer] = playerId;

        return realLevel6Function(pThis, a, b);
    }

    void writeBits(FILE* pFile, unsigned char* buffer, unsigned int numberOfBits)
    {
        int bitIndex = 0;
        int byteIndex = 0;
        fprintf(pFile, "bits: ");
        for (unsigned int i = 0; i < numberOfBits; i++)
        {
            char bit = ((buffer[byteIndex] >> bitIndex) & 1) ? '1' : '0';
            fwrite(&bit, 1, 1, pFile);
            bitIndex++;
            if (bitIndex == 8)
            {
                bitIndex = 0;
                byteIndex++;
            }
        }
        fprintf(pFile, "\n");
    }

    void __cdecl fakeLevel8Function(void* pWriteBuffer, int writeOffsetInBits, void* pReadBuffer, int readOffsetInBits, int bitsToCopy)
    {
        //OutputDebugString(L"tamods: fakeLevel8Function\n");
        bool modifyEdi = false;

        ClientData& clientData = clientMap[pWriteBuffer];

        // Each first level8 updates the bit count, each second level8 writes all the bits after the count.
        if (firstLevel8SinceLevel6)
        {
            firstLevel8SinceLevel6 = false;

            // Track sequence number of messages on channel 0
            unsigned int channel0data = *(unsigned int*)pReadBuffer;
            if ((channel0data & 0x00001ff8) == 0)
            {
                clientData.seqnr = (channel0data >> 13) & 0x1f;
            }

            if (gameEnded >= 2 && !clientData.fragmentInserted)
            {
                // Track writes to buffers and insert the fragment when possible

                if (clientData.previousWriteOffset == 0)
                {
                    clientData.previousWriteOffset = writeOffsetInBits;
                }
                else
                {
                    // If the offset for the first level8 changes, it means that this would be the
                    // first write for a new piece of data. That is when we can insert our own data.
                    if (writeOffsetInBits != clientData.previousWriteOffset)
                    {
                        clientData.fragmentInserted = true;
                        clientData.insertionOffset = writeOffsetInBits;
                    }
                }

            }
        }

        realLevel8Function(pWriteBuffer, writeOffsetInBits, pReadBuffer, readOffsetInBits, bitsToCopy);
        clientData.maxNumberOfBits = std::max(clientData.maxNumberOfBits, writeOffsetInBits + bitsToCopy);
    }

    void memMoveBits(unsigned char* pWriteBuffer, int fromOffsetInBits, int toOffsetInBits, int bitsToMove)
    {
        int fromByteIndex = fromOffsetInBits / 8;
        int fromBitIndex = fromOffsetInBits - fromByteIndex * 8;
        int lastToByteIndex = (toOffsetInBits + bitsToMove - 1) / 8;

        boost::dynamic_bitset<unsigned char> movedBits;
        movedBits.append(&pWriteBuffer[fromByteIndex], &pWriteBuffer[lastToByteIndex + 1]);

        boost::dynamic_bitset<unsigned char> beginning(movedBits.size());
        for (int i = 0; i < fromBitIndex; i++)
        {
            beginning[i] = movedBits[i];
            movedBits[i] = 0;
        }
        movedBits <<= (toOffsetInBits - fromOffsetInBits);
        movedBits |= beginning;

        std::vector<unsigned char> result(&pWriteBuffer[fromByteIndex], &pWriteBuffer[lastToByteIndex + 1]);
        boost::to_block_range(movedBits, result.begin());
        memcpy(&pWriteBuffer[fromByteIndex], result.data(), result.size());
    }

    int __stdcall fakeSendtoFunction(SOCKET s, char* buf, int len, int flags, const sockaddr* to, int tolen)
    {
        ClientData& clientData = clientMap[(void *)buf];
        if (clientData.fragmentInserted && !clientData.fragmentSent)
        {
            OverallMatchStats overallStats;
            PlayerMatchStats playerStats;

            if(bufferToPlayerId.count(buf) == 0)
            {
                Logger::error("Unable to find player ID for send buffer %08p", buf);
            }
            sStatsCollector.getSummary(bufferToPlayerId.at(buf), playerStats, overallStats);

#ifdef _DEBUG
            Logger::error("logging match summary stuff for player 0x%X", bufferToPlayerId.at(buf));
#endif

            char pBuffer[4096];

            // 0 (flag1 = 0)
            //  01 (flag1a = 2)
            //    0000000000 (channel = 0)
            unsigned int part1 = 0x00000004;
            unsigned int part1size = 13;

            //              01101 (counter = 22)
            unsigned int counter = clientData.seqnr + 1;
            unsigned int countersize = 5;

            //                   00000100
            unsigned int part2 = 0x00000020;
            unsigned int part2size = 8;

            //                                         10111000 (property = interestingproperty)
            unsigned int prop = 0x0000001d;
            unsigned int propsize = 8;

            unsigned int overallStatsSize = overallStats.size() * 8;
            unsigned int playerStatsSize = playerStats.size() * 8;

            //                                      0000011100000000000000001110 (prefix)
            unsigned int prefix1 = (overallStatsSize / 8) | (overallStatsSize / 8) << 19;
            unsigned int prefix1size = 28;

            unsigned int prefix2 = (playerStatsSize / 8) | (playerStatsSize / 8) << 19;
            unsigned int prefix2size = 28;

            //                           0001111100110 (payloadsize = 3320)
            //                                        x (object = FirstServerObject_0)
            unsigned int payloadsize = propsize + prefix1size + overallStatsSize +
                                       propsize + prefix2size + playerStatsSize;
            unsigned int payloadsizesize = (payloadsize > 3000) ? 13 : 14;

            clientData.insertionCount = part1size + countersize + part2size + payloadsizesize + payloadsize;

            memMoveBits((unsigned char *)buf, clientData.insertionOffset, clientData.insertionOffset + clientData.insertionCount, len * 8 - clientData.insertionOffset);

            len = (clientData.maxNumberOfBits + clientData.insertionCount) / 8 + 1;
            unsigned int writeOffset = clientData.insertionOffset;
            realLevel8Function(buf, writeOffset, &part1, 0, part1size);             writeOffset += part1size;

            realLevel8Function(buf, writeOffset, &counter, 0, countersize);         writeOffset += countersize;
            realLevel8Function(buf, writeOffset, &part2, 0, part2size);             writeOffset += part2size;
            realLevel8Function(buf, writeOffset, &payloadsize, 0, payloadsizesize); writeOffset += payloadsizesize;

            realLevel8Function(buf, writeOffset, &prop, 0, propsize);               writeOffset += propsize;
            realLevel8Function(buf, writeOffset, &prefix1, 0, prefix1size);         writeOffset += prefix1size;
            overallStats.toBytes(pBuffer);
            realLevel8Function(buf, writeOffset, &pBuffer, 0, overallStatsSize);    writeOffset += overallStatsSize;

            realLevel8Function(buf, writeOffset, &prop, 0, propsize);               writeOffset += propsize;
            realLevel8Function(buf, writeOffset, &prefix2, 0, prefix2size);         writeOffset += prefix2size;
            playerStats.toBytes(pBuffer);
            realLevel8Function(buf, writeOffset, &pBuffer, 0, playerStatsSize);     writeOffset += playerStatsSize;

            boost::dynamic_bitset<unsigned char> insertedbits;
            insertedbits.append(&buf[clientData.insertionOffset/8], &buf[writeOffset/8 + 1]);

#ifdef _DEBUG
            static char bitstring[65535] = {0};
            for (unsigned int i = 0; i < insertedbits.size(); i++)
            {
                bitstring[i] = insertedbits[i] ? '1' : '0';
            }
            Logger::error("Insertedbits are more or less: %s", bitstring);
#endif

            clientData.fragmentSent = true;
            clientData.insertionOffset = 0;
            clientData.insertionCount = 0;
        }
        clientData.maxNumberOfBits = 0;

        return realSendtoFunction(s, buf, len, flags, to, tolen);
    }

    void registerHooks()
    {
        realEndGameFunction = (EndGameFunction)DetourFunction((PBYTE)0xcdb110, (PBYTE)fakeEndGameFunction);
        realLevel6Function = (Level6Function)DetourFunction((PBYTE)0xc1b160, (PBYTE)fakeLevel6Function);
        realLevel8Function = (Level8Function)DetourFunction((PBYTE)0x42baf0, (PBYTE)fakeLevel8Function);
        HMODULE handle = GetModuleHandleA("ws2_32.dll");
        void* pSendto = GetProcAddress(handle, "sendto");
        realSendtoFunction = (SendtoFunction)DetourFunction((PBYTE)pSendto, (PBYTE)fakeSendtoFunction);
    }

    void unregisterHooks()
    {
        DetourRemove((PBYTE)realEndGameFunction, (PBYTE)&fakeEndGameFunction);
        DetourRemove((PBYTE)realLevel6Function, (PBYTE)&fakeLevel6Function);
        DetourRemove((PBYTE)realLevel8Function, (PBYTE)&fakeLevel8Function);
        DetourRemove((PBYTE)realSendtoFunction, (PBYTE)&fakeSendtoFunction);
    }

    void StatsCollector::addAccolade(int who, int accoladeId)
    {
        auto key = std::make_pair(who, accoladeId);
        auto it = mAccolades.find(key);
        if(it == mAccolades.end())
        {
            mAccolades[key] = 1;
        }
        else
        {
            it->second += 1;
        }
    }

    void StatsCollector::addToStat(int who, int statId, float value)
    {
        auto key = std::make_pair(who, statId);
        auto it = mStats.find(key);
        if(it == mStats.end())
        {
            mStats[key] = value;
        }
        else
        {
            it->second += value;
        }
    }

    void StatsCollector::updateStat(int who, int statId, float value)
    {
        mStats[std::make_pair(who, statId)] = value;
    }

    void StatsCollector::setField(int who, int fieldId, int value)
    {
        mFields[std::make_pair(who, fieldId)] = value;
    }

    float StatsCollector::GetMinTierValue(int statId)
    {
        auto it = cStatValueMap.find(statId);
        return (it == cStatValueMap.end()) ? 0.0f : it->second.tierlimit[0];
    }

    float StatsCollector::GetTieredWeight(int statId, float val)
    {
        float retVal = 0;

        auto it = cStatValueMap.find(statId);
        if (it != cStatValueMap.end())
        {
            if (val > it->second.tierlimit[2])
            {
                retVal = val * it->second.weight * 10000;
            }
            else if (val > it->second.tierlimit[1])
            {
                retVal = val * it->second.weight * 100;
            }
            else
            {
                retVal = val * it->second.weight;
            }
        }

        return retVal;
    }

    void StatsCollector::getSummary(int thisPlayerId, PlayerMatchStats &playerStats, OverallMatchStats &overallStats)
    {
        int available_overall_stats_slots = 4;
        int available_overall_accolade_slots = 6;
        int available_player_stats_slots = 8;
        int available_player_accolade_slots = 21;

        auto sortFunc = [this](const std::pair<std::pair<int, int>, float> &stat1,
                               const std::pair<std::pair<int, int>, float> &stat2)
        {
            return GetTieredWeight(stat1.first.second, stat1.second) > GetTieredWeight(stat2.first.second, stat2.second);
        };

        std::vector<std::pair<std::pair<int, int>, float>> sortedAccolades(mAccolades.begin(), mAccolades.end());
        std::sort(sortedAccolades.begin(), sortedAccolades.end(), sortFunc);

        std::vector<std::pair<std::pair<int, int>, float>> sortedStats(mStats.begin(), mStats.end());
        std::sort(sortedStats.begin(), sortedStats.end(), sortFunc);

#ifdef DEBUG
        Logger::error("Logging ordered accolades:");
        for(auto &stat: sortedAccolades)
        {
            Logger::error("  %d x %d for player %d with value %f", stat.first.second, (int)stat.second, stat.first.first, GetTieredWeight(stat.first.second, stat.second));
        }
        Logger::error("Logging ordered stats:");
        for(auto &stat: sortedStats)
        {
            Logger::error("  %d x %d for player %d with value %f", stat.first.second, (int)stat.second, stat.first.first, GetTieredWeight(stat.first.second, stat.second));
        }
#endif

        std::set<int> alreadyAddedStats;
        for(const auto &kv: sortedStats)
        {
            int statPlayerId = kv.first.first;
            int stat = kv.first.second;
            float value = kv.second;

            static const std::map<int,int> cStatIdToFieldId = {
                { CONST_STAT_AWD_KILLS, CONST_PLAYER_KILLS },
                { CONST_STAT_AWD_DEATHS, CONST_PLAYER_DEATHS },
                { CONST_STAT_CLASS_ASSISTS, CONST_PLAYER_ASSISTS },
                { CONST_STAT_AWD_CREDITS_EARNED, CONST_PLAYER_SCORE },
            };

            if(thisPlayerId == statPlayerId &&
               cStatIdToFieldId.count(stat) != 0)
            {
                setField(thisPlayerId, cStatIdToFieldId.at(stat), (int)value);
            }

            if(value < GetMinTierValue(stat))
            {
                continue;
            }

            if(available_overall_stats_slots > 0 && alreadyAddedStats.count(stat) == 0)
            {
                overallStats.addStatistic(statPlayerId, stat, value);
                available_overall_stats_slots--;
                alreadyAddedStats.insert(stat);
            }
            if(thisPlayerId == statPlayerId)
            {
                if(available_player_stats_slots > 0)
                {
                    playerStats.addStatistic(statPlayerId, stat, value);
                    available_player_stats_slots--;
                }
            }
        }

        std::set<int> alreadyAddedAccolades;
        for(const auto &kv: sortedAccolades)
        {
            int accPlayerId = kv.first.first;
            int acc = kv.first.second;
            float value = kv.second;

            if(value == 0.0)
            {
                continue;
            }

            if(available_overall_accolade_slots > 0 && alreadyAddedAccolades.count(acc) == 0)
            {
                overallStats.addAccolade(accPlayerId, acc, value);
                available_overall_accolade_slots--;
                alreadyAddedAccolades.insert(acc);
            }
            if(thisPlayerId == accPlayerId)
            {
                if(available_player_accolade_slots > 0)
                {
                    playerStats.addAccolade(accPlayerId, acc, value);
                    available_player_accolade_slots--;
                }
            }
        }

        const int overallFields[] =
        {
            CONST_UNKNOWN_04BE,
            CONST_MATCH_ID,
            CONST_MAP_DURATION,
            CONST_UNKNOWN_046C,
            CONST_UNKNOWN_02B4,
            CONST_MAP_ID,
            CONST_BE_SCORE,
            CONST_DS_SCORE
        };
        for(auto& fieldId : overallFields)
        {
            int value = 0;
            auto it = mFields.find(std::make_pair(0, fieldId));
            if(it != mFields.end())
            {
                value = it->second;
            }

            switch(s_typeMap.at(fieldId))
            {
            case FieldType::INT32:
                overallStats.addInt32(fieldId, value);
                break;
            case FieldType::INT64:
                overallStats.addInt64(fieldId, value);
                break;
            case FieldType::FLOAT:
                overallStats.addFloat(fieldId, (float)value);
                break;
            }
        }

        const int playerFields[] =
        {
            CONST_PLAYER_ID,
            CONST_PLAYER_KILLS,
            CONST_PLAYER_DEATHS,
            CONST_PLAYER_ASSISTS,
            CONST_PLAYER_SCORE,
            CONST_UNKNOWN_0095,
            CONST_STARTING_XP,
            CONST_UNKNOWN_04C7,
            CONST_BASE_XP,
            CONST_BONUS_XP,
            CONST_VIP_XP,
            CONST_XP_BAR_LENGTH,
        };
        for(auto& fieldId : playerFields)
        {
            int value = 0;
            auto it = mFields.find(std::make_pair(thisPlayerId, fieldId));
            if(it != mFields.end())
            {
                value = it->second;
            }

            switch(s_typeMap.at(fieldId))
            {
            case FieldType::INT32:
                playerStats.addInt32(fieldId, value);
                break;
            case FieldType::INT64:
                playerStats.addInt64(fieldId, value);
                break;
            case FieldType::FLOAT:
                playerStats.addFloat(fieldId, (float)value);
                break;
            }
        }
    }

}


#include "tests_config.h"

#include <CortidQCT/CortidQCT.h>

#include <Eigen/Core>
#include <gsl/gsl>
#include <gtest/gtest.h>
#include <igl/per_vertex_normals.h>

#include <cstdio>
#include <iterator>

using namespace CortidQCT;

#ifndef CortidQCT_DATADIR
#  error "No data dir given"
#endif

static std::string const mesh1 =
    std::string(CortidQCT_DATADIR) + "/SimpleVertebra.off";
static std::string const labels1 =
    std::string(CortidQCT_DATADIR) + "/SimpleVertebra-labels.txt";

TEST(Mesh, EmptyMeshIsMepty) {
  Mesh<double> const mesh;

  ASSERT_TRUE(mesh.isEmpty());
  ASSERT_EQ(0, mesh.vertexCount());
  ASSERT_EQ(0, mesh.triangleCount());
}

TEST(Mesh, LoadFromFileThrowsIfFileDesNotExist) {

  Mesh<double> mesh;

  // Generate temp file name
  auto const file = std::string{"some-non-existent-file.off"};

  ASSERT_THROW(mesh.loadFromFile(file, file), std::invalid_argument);
  ASSERT_THROW(mesh.loadFromFile(file), std::invalid_argument);
}

TEST(Mesh, WriteToFileDoesNotThrowOnEmptyMesh) {
  Mesh<double> const mesh;

  ASSERT_NO_THROW(mesh.writeToFile("does-not-matter.obj", "dummy.dat"));
  ASSERT_NO_THROW(mesh.writeToFile("does-not-matter.obj"));
}

TEST(Mesh, MeashLoadFromFileDoesNotThrowOnSuccess) {

  Mesh<double> mesh;

  ASSERT_NO_THROW(mesh.loadFromFile(mesh1, labels1));
  ASSERT_NO_THROW(mesh.loadFromFile(mesh1));
}

TEST(Mesh, MeshLoadSaveLoadPreservesMesh) {

  using namespace std::string_literals;
  using gsl::make_span;

  using IdxType = typename gsl::span<double>::index_type;

  Mesh<double> meshA, meshB;

  // load meshes
  ASSERT_NO_THROW(meshA.loadFromFile(mesh1, labels1));
  ASSERT_NO_THROW(meshB.loadFromFile(mesh1));

  std::string const filenameAMesh = std::tmpnam(nullptr) + ".off"s;
  std::string const filenameALabel = std::tmpnam(nullptr) + ".txt"s;
  std::string const filenameBMesh = std::tmpnam(nullptr) + ".off"s;

  // save meshes
  ASSERT_NO_THROW(meshA.writeToFile(filenameAMesh, filenameALabel));
  ASSERT_NO_THROW(meshB.writeToFile(filenameBMesh));

  Mesh<double> meshC, meshD;

  // load meshes
  ASSERT_NO_THROW(meshC.loadFromFile(filenameAMesh, filenameALabel));
  ASSERT_NO_THROW(meshD.loadFromFile(filenameBMesh));

  // assert equality of counts
  ASSERT_EQ(meshA.vertexCount(), meshC.vertexCount());
  ASSERT_EQ(meshA.triangleCount(), meshC.triangleCount());
  ASSERT_EQ(meshB.vertexCount(), meshD.vertexCount());
  ASSERT_EQ(meshB.triangleCount(), meshD.triangleCount());
  ASSERT_EQ(meshA.vertexCount(), meshD.vertexCount());

  // assert equality of vertices
  auto const nFloats = gsl::narrow_cast<IdxType>(3 * meshA.vertexCount());
  meshC.withUnsafeVertexPointer([&meshA, nFloats](auto const pV) {
    meshA.withUnsafeVertexPointer([pV, nFloats](auto const pU) {
      auto pvi = pV;
      auto const spanA = make_span(pU, nFloats);

      for (auto &&ui : spanA) {
        ASSERT_DOUBLE_EQ(ui, *pvi);
        ++pvi;
      }
    });
  });

  meshD.withUnsafeVertexPointer([&meshB, nFloats](auto const pV) {
    meshB.withUnsafeVertexPointer([pV, nFloats](auto const pU) {
      auto pvi = pV;
      auto const spanA = make_span(pU, nFloats);

      for (auto &&ui : spanA) {
        ASSERT_DOUBLE_EQ(ui, *pvi);
        ++pvi;
      }
    });
  });

  // assert equality of triangles
  ASSERT_TRUE(meshC.withUnsafeIndexPointer([&meshA](auto const pI) {
    return meshA.withUnsafeIndexPointer(
        [pI, size = meshA.triangleCount() * 3](auto const pJ) {
          return std::equal(pI, pI + size, pJ);
        });
  }));
  ASSERT_TRUE(meshD.withUnsafeIndexPointer([&meshB](auto const pI) {
    return meshB.withUnsafeIndexPointer(
        [pI, size = meshB.triangleCount() * 3](auto const pJ) {
          return std::equal(pI, pI + size, pJ);
        });
  }));

  // assert equality of labels
  ASSERT_TRUE(meshC.withUnsafeLabelPointer([&meshA](auto const pI) {
    return meshA.withUnsafeLabelPointer(
        [pI, size = meshA.vertexCount()](auto const pJ) {
          return std::equal(pI, pI + size, pJ);
        });
  }));
  ASSERT_TRUE(meshD.withUnsafeLabelPointer([&meshB](auto const pI) {
    return meshB.withUnsafeLabelPointer(
        [pI, size = meshB.vertexCount()](auto const pJ) {
          return std::equal(pI, pI + size, pJ);
        });
  }));
}

TEST(Mesh, NormalsOrientedOutwards) {
  using Eigen::Index;
  using Eigen::Map;
  using Eigen::MatrixXf;
  using Eigen::Vector3f;
  using Eigen::VectorXf;
  using FMatrix =
      Eigen::Matrix<Mesh<float>::Index, Eigen::Dynamic, Eigen::Dynamic>;

  Mesh<float> mesh;
  ASSERT_NO_THROW(mesh.loadFromFile(mesh1));

  MatrixXf const V = mesh.withUnsafeVertexPointer([&](float const *vPtr) {
    return Map<MatrixXf const>{vPtr, 3, gsl::narrow<Index>(mesh.vertexCount())}
        .transpose();
  });

  FMatrix const F = mesh.withUnsafeIndexPointer([&](auto const *iPtr) {
    return Map<FMatrix const>{iPtr, 3, gsl::narrow<Index>(mesh.triangleCount())}
        .transpose();
  });

  // compute centroid
  Vector3f const centroid = V.transpose().rowwise().mean();

  MatrixXf N;
  igl::per_vertex_normals(V, F, igl::PER_VERTEX_NORMALS_WEIGHTING_TYPE_ANGLE,
                          N);

  MatrixXf const dotProductMatrix =
      N.array() * (V.rowwise() - centroid.transpose()).array();
  VectorXf const dotProducts =
      dotProductMatrix.array().rowwise().sum() /
      dotProductMatrix.array().square().rowwise().sum();

  ASSERT_TRUE((dotProducts.array() > -0.25f).all());
}

#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wconversion"

template <class T> class MeshQueriesTest : public ::testing::Test {
protected:
  Mesh<T> mesh;

  void SetUp() override {
    mesh = Mesh<T>{4, 4};

    // Build a regular tetrahedron
    // set vertices
    mesh.withUnsafeVertexPointer([](auto *vertices) {
      vertices[0] = T{0.9428090416}; // sqrt(8/9)
      vertices[1] = T{0};
      vertices[2] = T{-0.3333333333}; // -1/3

      vertices[3] = T{-0.4714045208}; // -sqrt(2/9)
      vertices[4] = T{0.8164965809};  // sqrt(2/3)
      vertices[5] = T{-0.3333333333}; // -1/3

      vertices[6] = T{-0.4714045208}; // -sqrt(2/9)
      vertices[7] = T{-0.8164965809}; // -sqrt(2/3)
      vertices[8] = T{-0.3333333333}; // -1/3

      vertices[9] = T{0};
      vertices[10] = T{0};
      vertices[11] = T{1};
    });

    // set indices
    mesh.withUnsafeIndexPointer([](auto *indices) {
      indices[0] = 0;
      indices[1] = 3;
      indices[2] = 1;

      indices[3] = 1;
      indices[4] = 3;
      indices[5] = 2;

      indices[6] = 0;
      indices[7] = 2;
      indices[8] = 3;

      indices[9] = 0;
      indices[10] = 1;
      indices[11] = 2;
    });
  }
};

typedef ::testing::Types<float, double> MeshTypes;
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wgnu-zero-variadic-macro-arguments"
TYPED_TEST_CASE(MeshQueriesTest, MeshTypes);
#pragma clang diagnostic pop

TYPED_TEST(MeshQueriesTest, BarycentricToCartesianSingleCall) {
  using T = TypeParam;
  using BC = BarycentricPoint<T, typename Mesh<T>::Index>;

  constexpr T oneThird = 1.0 / 3.0;
  constexpr std::array<T, 3> refPoint = {T{0.1571348403}, T{0.272165527},
                                         T{0.1111111111}};

  auto const center = BC{{oneThird, oneThird}, 0};

  auto const testPoint = this->mesh.cartesianRepresentation(center);

  ASSERT_NEAR(refPoint[0], testPoint[0], 1e-6);
  ASSERT_NEAR(refPoint[1], testPoint[1], 1e-6);
  ASSERT_NEAR(refPoint[2], testPoint[2], 1e-6);
}

TYPED_TEST(MeshQueriesTest, BarycentricToCartesianSequenceCall) {
  using T = TypeParam;
  using BC = BarycentricPoint<T, typename Mesh<T>::Index>;

  constexpr T oneThird = 1.0 / 3.0;

  std::vector<BC> const query = {
      BC{{oneThird, oneThird}, 0}, BC{{oneThird, oneThird}, 1},
      BC{{oneThird, oneThird}, 2}, BC{{oneThird, oneThird}, 3}};

  std::vector<std::array<T, 3>> const refPoints = {
      {0.1571348403, 0.272165527, 0.1111111111},
      {-0.3142696805, 0, 0.1111111111},
      {0.1571348403, -0.272165527, 0.1111111111},
      {0, 0, -0.3333333333}};

  auto const testPoints = this->mesh.cartesianRepresentation(query);

  ASSERT_EQ(refPoints.size(), testPoints.size());
  for (auto i = 0u; i < testPoints.size(); ++i) {
    auto const &rp = refPoints[i];
    auto const &tp = testPoints[i];

    ASSERT_NEAR(rp[0], tp[0], 1e-6);
    ASSERT_NEAR(rp[1], tp[1], 1e-6);
    ASSERT_NEAR(rp[2], tp[2], 1e-6);
  }
}

TYPED_TEST(MeshQueriesTest, RayMeshIntersectionSingleCall) {
  using T = TypeParam;
  using R = Ray<T>;

  R const ray = {{0, 0, -1}, {0, 0, 1}};

  auto const intersection = this->mesh.rayIntersection(ray);

  ASSERT_TRUE(std::isfinite(intersection.signedDistance));
  ASSERT_NEAR(0.666666666666, intersection.signedDistance, 1e-6);
  ASSERT_EQ(3, intersection.position.triangleIndex);
  ASSERT_NEAR(0.33333333333, intersection.position.uv[0], 1e-6);
  ASSERT_NEAR(0.33333333333, intersection.position.uv[1], 1e-6);
}

TYPED_TEST(MeshQueriesTest, RayMeshIntersectionSequence) {
  using T = TypeParam;
  using R = Ray<T>;

  std::array<R, 2> const rays = {
      {{{0, 0, -1}, {0, 0, 1}}, {{4, 0, -1}, {0, 0, 1}}}};

  std::vector<RayMeshIntersection<T>> intersections;

  this->mesh.rayIntersections(rays.data(), rays.data() + 2,
                              std::back_inserter(intersections));

  ASSERT_EQ(2, intersections.size());
  ASSERT_TRUE(std::isfinite(intersections[0].signedDistance));
  ASSERT_FALSE(std::isfinite(intersections[1].signedDistance));
  ASSERT_NEAR(0.666666666666, intersections[0].signedDistance, 1e-6);
  ASSERT_EQ(3, intersections[0].position.triangleIndex);
  ASSERT_NEAR(0.33333333333, intersections[0].position.uv[0], 1e-6);
  ASSERT_NEAR(0.33333333333, intersections[0].position.uv[1], 1e-6);
}

#pragma clang diagnostic pop


#ifndef INCLUDE_FOOTSTEPAFFORDANCE_HPP_
#define INCLUDE_FOOTSTEPAFFORDANCE_HPP_

#include <octomap/octomap.h>
#include <Eigen/Dense>
#include <features.hpp>
#include <map>
#include <ros/ros.h>

namespace costmap
{

class FootstepAffordance
{
    public:

        FootstepAffordance();

        ~FootstepAffordance();

        unsigned int computeMeanAndCovariance(Eigen::Vector3d& meanPosition, Eigen::Matrix3d& covarianceMatrix, const std::vector<Eigen::Vector3d>& cloud);

        void solvePlaneParameters(Eigen::Vector3d &normal, double &curvature, const Eigen::Matrix3d &covarianceMatrix);

        void computeRoots(Eigen::Vector3d& roots, const Eigen::Matrix3d& m);

        void computeRoots2(Eigen::Vector3d& roots, const Eigen::Matrix3d::Scalar& b, const Eigen::Matrix3d::Scalar& c);

        void regionOfInterest();

        void run(octomap::OcTree* octomap,
                const Eigen::Vector3d& robotStateXYZ,
                const Eigen::Vector3d& robotStateRPY,
                std::map<std::pair<double, double>, double> &hMap,
                std::map<std::pair<double, double>, double> &cMap);

        void calculateCost(octomap::OcTree* octomap, const octomap::OcTreeKey& surfaceCellKey, std::map<std::pair<double, double>, double> &cMap);

        void addToHeightmap(Eigen::Vector3d surfaceCellXYZ, std::map<std::pair<double, double>, double> &hMap);

        void addToCostmap(Eigen::Vector3d surfaceCellXYZ, double &totalCost, std::map<std::pair<double, double>, double> &cMap);

    private:

        const double distanceFromRobot = 0.3;
        double maxX = 1.0;
        double minX = 0.24;
        double maxY = 0.18;
        double minY = -0.18;
        double maxZ = 0.4;
        double minZ = -0.4;
        TerrainData terrainParameters;
        int depth_ = 16;
        bool firstRun_ = false;
        int neighbourMaxX, neighbourMaxY, neighbourMaxZ = 1;
        int neighbourMinX, neighbourMinY, neighbourMinZ = -1;
        terrainFeature::Feature features;
};

}

#endif  //  INCLUDE_FOOTSTEPAFFORDANCE_HPP_


#include <cstdint>
#include <iostream>
#include <boost/asio.hpp>
#include "networking.h"
#include "gene.h"
#include "aux.h"
using namespace std;
using namespace boost::asio;
using namespace seal;
#define threhold 150
int main(){
    vector<float_t> inp;
    size_t iput_size;
    unsigned short port=9999;
    size_t ploy_modulus_degree =8192;
    //inital network interface
    io_context iocontext;
    ip::tcp::acceptor acceptor(iocontext);
    ip::tcp::endpoint endp(ip::tcp::v4(),port);
    acceptor.open(endp.protocol());
    acceptor.set_option(ip::tcp::acceptor::reuse_address(true));
    acceptor.bind(endp);
    acceptor.listen();
    cout << "listening"<<endl;
    ip::tcp::socket socket(iocontext);
    acceptor.accept(socket);
    Networking net(socket);
    GeneParams param(ploy_modulus_degree,ckks);
    SecretKey secret_key;
    net.set_seal_context(param.context);

    cout<<"[+]connection established,sending hello to the client"<<endl;
    net.send_command();
    cout<<"[+]waiting for hello back.."<<endl;
    net.recv_command();
    //cout << "waiting for set size"<<endl;
    //size_t server_size=net.read_uint32();

    cout<<"[+]receiving user's public key"<<endl;
    PublicKey recv_pk;
    net.read_public_key(recv_pk);

    cout << "[+]waitint for relinerize key"<<endl;
    RelinKeys recv_relin_key;
    net.read_relin_key(recv_relin_key);

    cout<< "[+]waiting for encrypted RNA expressing "<<endl;
    vector<Ciphertext> recv_RNA;
    net.read_enc_geneRNAs(recv_RNA);
    cout<<recv_RNA[0].size()<<endl;
    cout<<"[+]received User data"<<endl;

    //evaluate distance
    Evaluator evaluator(param.context);
    Encryptor encryptor(param.context,recv_pk);

    //CKKSEncoder encoder(param.context);
    vector<double> refer;
    double curr_point = 0;

    for (size_t i = 0; i < ploy_modulus_degree; i++, curr_point += 0.01)
    {
        refer.push_back(curr_point);
    }

    print_vector(refer);
    vector<Ciphertext> enc_refer;
    vector<double> slice;
    size_t step=ploy_modulus_degree/2;
    size_t i=0;
    while(i<refer.size()/step){
        if((refer.end()-(refer.begin()+(i+1)*step))<step)
            slice.assign(refer.begin()+i*step,refer.end());

        else{
        slice.assign(refer.begin()+i*step,refer.begin()+(i+1)*step);
        }
        enc_refer.push_back(param.encrypted_genedata(slice,encryptor,ckks));
        evaluator.sub(enc_refer[i],recv_RNA[i],enc_refer[i]);
        evaluator.square_inplace(enc_refer[i]);
        slice.clear();

        i+=1;

    }
    net.write_enc_geneRNAs(enc_refer);
    //processing encrypted distance that are send back
    net.read_public_key(recv_pk);
    Ciphertext final_distance;
    net.read_enc_geneRNA(final_distance);
    vector<uint32_t> recv_distance;
    GeneParams params(ploy_modulus_degree,bfv);
    Evaluator eval(params.context);
    Plaintext plain_dis(to_string(threhold));
    Encryptor thre_encryptor(params.context,recv_pk);
    Ciphertext enc_threhold;
    thre_encryptor.encrypt(plain_dis,enc_threhold);
    evaluator.sub_inplace(final_distance,enc_threhold);

    net.write_enc_geneRNA(final_distance);
    return 0;
}