//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2014   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2014 MetaScale SAS
//
//          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_CONSTANT_CONSTANTS_SIMD_VMX_ALTIVEC_MSEVEN_HPP_INCLUDED
#define BOOST_SIMD_CONSTANT_CONSTANTS_SIMD_VMX_ALTIVEC_MSEVEN_HPP_INCLUDED
#ifdef BOOST_SIMD_HAS_VMX_SUPPORT

#include <boost/simd/constant/constants/mseven.hpp>
#include <boost/dispatch/attributes.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT          ( Mseven
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((target_ < simd_ < int8_<A0>
                                                        , boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A0::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return vec_splat_s8(-7);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( Mseven
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((target_ < simd_ < uint8_<A0>
                                                        , boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A0::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return vec_splat_u8(-7);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( Mseven
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((target_ < simd_ < int16_<A0>
                                                        , boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A0::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return vec_splat_s16(-7);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( Mseven
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((target_ < simd_ < uint16_<A0>
                                                        , boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A0::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return vec_splat_u16(-7);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( Mseven
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((target_ < simd_ < int32_<A0>
                                                        , boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A0::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return vec_splat_s32(-7);
    }
  };

  BOOST_DISPATCH_IMPLEMENT          ( Mseven
                                    , boost::simd::tag::vmx_
                                    , (A0)
                                    , ((target_ < simd_ < uint32_<A0>
                                                        , boost::simd::tag::vmx_
                                                        >
                                                >
                                      ))
                                    )
  {
    typedef typename A0::type result_type;

    BOOST_FORCEINLINE result_type operator()(A0 const&) const
    {
      return vec_splat_u32(-7);
    }
  };
} } }

#endif
#endif


#include <iostream>

#include <boost/numeric/linear_algebra/operators.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/linear_algebra/concepts.hpp>

#include <libs/numeric/linear_algebra/test/algebraic_functions.hpp>

using math::identity; using math::add;

using std::string;

namespace math {
    template<>
    struct identity_t< math::add<string>, string >
    {
        string operator()(const math::add<string>&, const string&) const
        {
	    return string();
        }
    } ;

# ifdef __GXX_CONCEPTS__
    concept_map Monoid< math::add<string>, string > {}
# endif // __GXX_CONCEPTS__
}

int main(int, char* [])
{
    using namespace std;
    using namespace mtl;

    string               sa("a"), sab("ab"), sbc("bc"), sc("c"), s;
    math::add<string>    string_add;

    cout << "equal_results(sa, sbc,  sab, sc, string_add) "
	 << equal_results(sa, sbc,  sab, sc, string_add)  << endl;
    cout << "equal_results(sab, sbc,  sab, sc, string_add) "
	 << equal_results(sab, sbc,  sab, sc, string_add) << endl;

    cout << "identity_pair(s, s, string_add) "
	 << identity_pair(s, s, string_add)  << endl;
    cout << "identity_pair(sa, sa, string_add) "
	 << identity_pair(sa, sa, string_add)  << endl;

    cout << "multiply_and_square(sab, 13, string_add) "
	 << multiply_and_square(sab, 13, string_add) << endl;
    return 0;
}


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

#ifndef _pair_hmm_hpp
#define _pair_hmm_hpp

#if __GNUC__ >= 6
    #pragma GCC diagnostic ignored "-Wignored-attributes"
#endif

#include <cstddef>
#include <algorithm>
#include <iterator>
#include <cassert>
#include <limits>
#include <vector>
#include <emmintrin.h>
#include <immintrin.h>

#include <boost/align/aligned_allocator.hpp>

namespace octopus { namespace hmm { namespace simd {

template <typename InstructionSet,
          template <class> class InitializerType>
class PairHMM : private InstructionSet
{
public:
    using ScoreType = typename InstructionSet::ScoreType;

private:
    using VectorType  = typename InstructionSet::VectorType;
    using Initializer = InitializerType<InstructionSet>;
    using SmallVector = std::vector<VectorType, boost::alignment::aligned_allocator<VectorType>>;

    struct NullType {};

    // Methods
    using InstructionSet::vectorise;
    using InstructionSet::vectorise_zero_set_last;
    using InstructionSet::_extract;
    using InstructionSet::_insert_bottom;
    using InstructionSet::_insert_top;
    using InstructionSet::_add;
    using InstructionSet::_and;
    using InstructionSet::_andnot;
    using InstructionSet::_or;
    using InstructionSet::_cmpeq;
    using InstructionSet::_min;
    using InstructionSet::_max;
    using InstructionSet::_left_shift_word;
    using InstructionSet::_right_shift_word;

    // Constants
    constexpr static const char* name_ = InstructionSet::name;
    constexpr static int band_size_ {InstructionSet::band_size};
    constexpr static ScoreType infinity_tolerance_ {0x7FF};
    constexpr static ScoreType infinity_ {std::numeric_limits<ScoreType>::max() - infinity_tolerance_};
    constexpr static int trace_bits_ {2};
    constexpr static ScoreType n_score_ {2 << trace_bits_};

    constexpr static ScoreType max_quality_score_ {64}; // maximum reasonable phred base quality
    constexpr static ScoreType null_score_ {std::numeric_limits<ScoreType>::min()}; // baseline for score zero

    constexpr static int match_label_  {0};
    constexpr static int insert_label_ {1};
    constexpr static int delete_label_ {3};

    constexpr auto vectorise(NullType) const noexcept { return NullType {}; }

    template <int n, typename T>
    static auto vectorise_left_shift_bits(const T* values) noexcept
    {
        return _left_shift_bits<n>(vectorise(values));
    }
    template <int n>
    static auto vectorise_left_shift_bits(const std::int8_t value) noexcept
    {
        return vectorise(value << n);
    }
    template <int n>
    constexpr auto vectorise_left_shift_bits(NullType) const noexcept { return NullType {}; }
    template <int idx>
    static auto _right_shift_bits(const VectorType& vec) noexcept
    {
        return InstructionSet::template _right_shift_bits<idx>(vec);
    }
    template <int idx>
    static auto _left_shift_bits(const VectorType& vec) noexcept
    {
        return InstructionSet::template _left_shift_bits<idx>(vec);
    }

    VectorType maybe_right_shift_word(const VectorType& vec, const std::int8_t*) const noexcept
    {
        return _right_shift_word(vec);
    }
    const VectorType& maybe_right_shift_word(const VectorType& vec, ScoreType) const noexcept
    {
        return vec;
    }

    void update_gap_penalty(VectorType& current, const std::int8_t* source, const std::size_t gap_idx) const noexcept
    {
        current = _insert_top(_right_shift_word(current), source[gap_idx] << trace_bits_);
    }
    void update_gap_penalty(VectorType& current, const ScoreType source, const std::size_t gap_idx) const noexcept {}

    void
    update_snv_mask(VectorType& curr_mask,
                    VectorType& curr_caps,
                    const char* snv_mask,
                    const std::int8_t* caps,
                    const int pos,
                    const int truth_len) const noexcept
    {
        const bool pos_in_range {pos < truth_len};
        curr_mask = _insert_top(_right_shift_word(curr_mask), pos_in_range ? snv_mask[pos] : 'N');
        curr_caps = _insert_top(_right_shift_word(curr_caps), (pos_in_range ? caps[pos] : infinity_) << trace_bits_);
    }
    void update_snv_mask(NullType, NullType, NullType, NullType, int, int) const noexcept {}

    void
    update_match_state(VectorType& current,
                       const VectorType& _targetwin,
                       const VectorType& _truthwin,
                       const VectorType& _qualitieswin,
                       const VectorType& _truthnqual,
                       const VectorType& _snvmaskwin,
                       const VectorType& _snv_priorwin) const noexcept
    {
        auto _snvmask = _cmpeq(_targetwin, _snvmaskwin);
        current = _add(current, _min(_andnot(_cmpeq(_targetwin, _truthwin), _min(_qualitieswin, _or(_and(_snvmask, _snv_priorwin), _andnot(_snvmask, _qualitieswin)))), _truthnqual));
    }
    void
    update_match_state(VectorType& current,
                       const VectorType& _targetwin,
                       const VectorType& _truthwin,
                       const VectorType& _qualitieswin,
                       const VectorType& _truthnqual,
                       NullType, NullType) const noexcept
    {
        current = _add(current, _min(_andnot(_cmpeq(_targetwin, _truthwin), _qualitieswin), _truthnqual));
    }

    auto make_traceback_array(int target_len, int) const noexcept { return SmallVector(2 * (target_len + band_size_) + 1); }
    auto make_traceback_array(int target_len, NullType) const noexcept { return NullType {}; }

    void
    update_traceback(SmallVector& _backpointers,
                     const int index,
                     VectorType& match_state,
                     VectorType& insert_state,
                     VectorType& delete_state) const noexcept
    {
        const static VectorType _three = vectorise(3);
        const static VectorType _one = _right_shift_bits<1>(_three);   // could save one register
        _backpointers[index] = _or(_or(_and(_three, match_state), _left_shift_bits<2 * insert_label_>(_and(_three, insert_state))),
                               _left_shift_bits<2 * delete_label_>(_and(_three, delete_state)));
        // set state labels
        match_state = _andnot(_three, match_state);
        insert_state = _or(_andnot(_three, insert_state), _one);
        delete_state = _or(_andnot(_three, delete_state), _three);
    }
    void update_traceback(NullType, int, VectorType&, VectorType&, VectorType&) const noexcept {}

    void
    set_alignments(const char* truth,
                   const char* target,
                   const int truth_len,
                   const int target_len,
                   SmallVector& _backpointers,
                   const int minscoreidx,
                   int& first_pos,
                   char* align1,
                   char* align2) const noexcept
    {
        if (minscoreidx < 0) {
            // minscore was never updated so we must have overflowed badly
            first_pos = -1;
        }
        auto s = minscoreidx;    // point to the dummy match transition
        auto i      = s / 2 - target_len;
        auto y      = target_len;
        auto x      = s - y;
        auto alnidx = 0;
        auto ptr = reinterpret_cast<ScoreType*>(_backpointers.data() + s);
        if ((ptr + i) < reinterpret_cast<ScoreType*>(_backpointers.data())
            || (ptr + i) >= reinterpret_cast<ScoreType*>(_backpointers.data() + _backpointers.size())) {
            first_pos = -1;
            return;
        }
        auto state = (ptr[i] >> (2 * match_label_)) & 3;
        s -= 2;
        // this is 2*y (s even) or 2*y+1 (s odd)
        while (y > 0) {
            if (s < 0 || i < 0) {
                // This should never happen so must have overflowed
                first_pos = -1;
                return;
            }
            const auto new_state = (reinterpret_cast<ScoreType*>(_backpointers.data() + s)[i] >> (2 * state)) & 3;
            if (state == match_label_) {
                s -= 2;
                align1[alnidx] = truth[--x];
                align2[alnidx] = target[--y];
            } else if (state == insert_label_) {
                i += s & 1;
                s -= 1;
                align1[alnidx] = gap_label;
                align2[alnidx] = target[--y];
            } else {
                s -= 1;
                i -= s & 1;
                align1[alnidx] = truth[--x];
                align2[alnidx] = gap_label;
            }
            state = new_state;
            alnidx++;
        }
        align1[alnidx] = 0;
        align2[alnidx] = 0;
        first_pos = x;
        // reverse them
        for (int j {alnidx - 1}, i = 0; i < j; ++i, j--) {
            x = align1[i];
            y = align2[i];
            align1[i] = align1[j];
            align2[i] = align2[j];
            align1[j] = x;
            align2[j] = y;
        }
    }
    void set_alignments(const char*, const char*, int, int, NullType, int, NullType, NullType, NullType) const noexcept {}

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant,
              typename SnvMaskArrayOrNull,
              typename SnvBaseQualityCapArrayOrNull,
              typename PositionOrNull,
              typename CharArrayOrNull>
    int
    align_helper(const char* truth,
                 const char* target,
                 const std::int8_t* qualities,
                 const int truth_len,
                 const int target_len,
                 const OpenPenaltyArrayOrConstant gap_open,
                 const ExtendPenaltyArrayOrConstant gap_extend,
                 const SnvMaskArrayOrNull snv_mask,
                 const SnvBaseQualityCapArrayOrNull snv_prior,
                 const ScoreType nuc_prior,
                 PositionOrNull& first_pos,
                 CharArrayOrNull align1,
                 CharArrayOrNull align2) const noexcept
    {
        assert(target_len > 0 && truth_len > band_size_ && (truth_len == target_len + 2 * band_size_ - 1));
        const static VectorType _inf = vectorise(infinity_);
        const auto _nuc_prior = vectorise_left_shift_bits<trace_bits_>(nuc_prior);
        auto _truthwin     = vectorise(truth);
        auto _targetwin    = _inf;
        auto _qualitieswin = vectorise_left_shift_bits<trace_bits_>(max_quality_score_);
        auto _gap_open     = vectorise_left_shift_bits<trace_bits_>(gap_open);
        auto _gap_extend   = vectorise_left_shift_bits<trace_bits_>(gap_extend);
        auto _snvmaskwin   = vectorise(snv_mask);
        auto _snv_priorwin = vectorise_left_shift_bits<trace_bits_>(snv_prior);
        auto _truthnqual   = _add(_and(_cmpeq(_truthwin, vectorise('N')), vectorise(n_score_ - infinity_)), _inf);
        auto _backpointers = make_traceback_array(target_len, first_pos);
        auto _m1 = _inf, _i1 = _inf, _d1 = _inf, _m2 = _inf, _i2 = _inf, _d2 = _inf;
        Initializer rollinginit {null_score_};
        ScoreType minscore {infinity_}, cur_score;
        int minscoreidx {-1};
        for (int s = 0; s < 2 * (target_len + band_size_); s += 2) {
            // s even. truth is current; target needs updating
            _targetwin    = _left_shift_word(_targetwin);
            _qualitieswin = _left_shift_word(_qualitieswin);
            if (s / 2 < target_len) {
                _targetwin    = _insert_bottom(_targetwin, target[s / 2]);
                _qualitieswin = _insert_bottom(_qualitieswin, qualities[s / 2] << trace_bits_);
            } else {
                _targetwin    = _insert_bottom(_targetwin, '0');
                _qualitieswin = _insert_bottom(_qualitieswin, max_quality_score_ << trace_bits_);
            }
            _m1 = rollinginit.init(_m1);
            _m2 = rollinginit.init(_m2);
            _m1 = _min(_m1, _min(_i1, _d1));
            if (s / 2 >= target_len) {
                cur_score = _extract(_m1, s / 2 - target_len);
                if (cur_score < minscore) {
                    minscore = cur_score;
                    minscoreidx = s;
                }
            }
            update_match_state(_m1, _targetwin, _truthwin, _qualitieswin, _truthnqual, _snvmaskwin, _snv_priorwin);
            _d1 = _min(_add(_d2, maybe_right_shift_word(_gap_extend, gap_extend)), _add(_min(_m2, _i2), maybe_right_shift_word(_gap_open, gap_open))); // allow I->D
            _d1 = _insert_bottom(_left_shift_word(_d1), infinity_);
            _i1 = _add(_min(_add(_i2, _gap_extend), _add(_m2, _gap_open)), _nuc_prior);
            update_traceback(_backpointers, s, _m1, _i1, _d1);
            // S odd. Truth needs updating; target is current
            const auto pos = band_size_ + s / 2;
            const bool pos_in_range {pos < truth_len};
            const char base {pos_in_range ? truth[pos] : 'N'};
            _truthwin   = _insert_top(_right_shift_word(_truthwin), base);
            _truthnqual = _insert_top(_right_shift_word(_truthnqual), base == 'N' ? n_score_ : infinity_);
            const auto gap_idx = pos_in_range ? pos : truth_len - 1;
            update_gap_penalty(_gap_open, gap_open, gap_idx);
            update_gap_penalty(_gap_extend, gap_extend, gap_idx);
            update_snv_mask(_snvmaskwin, _snv_priorwin, snv_mask, snv_prior, pos, truth_len);
            rollinginit.update();
            _m2 = _min(_m2, _min(_i2, _d2));
            if (s / 2 >= target_len) {
                cur_score = _extract(_m2, s / 2 - target_len);
                if (cur_score < minscore) {
                    minscore = cur_score;
                    minscoreidx = s + 1;
                }
            }
            update_match_state(_m2, _targetwin, _truthwin, _qualitieswin, _truthnqual, _snvmaskwin, _snv_priorwin);
            _d2 = _min(_add(_d1, _gap_extend), _add(_min(_m1, _i1), _gap_open)); // allow I->D
            _i2 = _insert_top(_add(_min(_add(_right_shift_word(_i1), _gap_extend),
                                        _add(_right_shift_word(_m1), _gap_open)), _nuc_prior), infinity_);
            update_traceback(_backpointers, s + 1, _m2, _i2, _d2);
        }
        set_alignments(truth, target, truth_len, target_len, _backpointers, minscoreidx, first_pos, align1, align2);
        return (minscore - null_score_) >> trace_bits_;
    }

    auto get(const std::int8_t* values, int index) const noexcept { return values[index]; }
    auto get(std::int8_t value, int index) const noexcept { return value; }
    auto
    get_mismatch_quality(const char* target,
                         const std::int8_t* quals,
                         int x, int y,
                         const char* snv_mask,
                         const std::int8_t* caps) const noexcept
    {
        return (snv_mask[x] == target[y]) ? std::min(quals[y], caps[x]) : quals[y];
    }
    auto
    get_mismatch_quality(const char* target,
                         const std::int8_t* quals,
                         int x, int y,
                         NullType,
                         NullType) const noexcept
    {
        return quals[y];
    }

    template <typename CharArrayOrNull,
              typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant,
              typename SnvMaskArrayOrNull,
              typename SnvBaseQualityCapArrayOrNull>
    int
    calculate_flank_score_helper(const int truth_len,
                                 const int lhs_flank_len,
                                 const int rhs_flank_len,
                                 const CharArrayOrNull target,
                                 const std::int8_t* quals,
                                 const OpenPenaltyArrayOrConstant gap_open,
                                 const ExtendPenaltyArrayOrConstant gap_extend,
                                 const SnvMaskArrayOrNull snv_mask,
                                 const SnvBaseQualityCapArrayOrNull snv_prior,
                                 const ScoreType nuc_prior,
                                 const int first_pos,
                                 const char* alignment1,
                                 const char* alignment2,
                                 int& target_mask_size) const noexcept
    {
        static constexpr char match {'M'}, insertion {'I'}, deletion {'D'};
        auto prev_state = match;
        int truth_idx {first_pos}, target_idx {0}, alignment_idx {0};
        int result {0};    // alignment score (within flank)
        const auto rhs_flank_begin = truth_len - rhs_flank_len;
        target_mask_size = 0;
        while (alignment1[alignment_idx]) {
            auto new_state = match;
            if (alignment1[alignment_idx] == gap_label) {
                new_state = insertion;
            } else if (alignment2[alignment_idx] == gap_label) { // can't be both '-'
                new_state = deletion;
            }
            const bool in_flank {truth_idx < lhs_flank_len || truth_idx >= rhs_flank_begin};
            switch (new_state) {
                case match:
                {
                    if (in_flank) {
                        if (alignment1[alignment_idx] != alignment2[alignment_idx]) {
                            if (alignment1[alignment_idx] != 'N') {
                                result += get_mismatch_quality(target, quals, truth_idx, target_idx, snv_mask, snv_prior);
                            } else {
                                result += n_score_ >> trace_bits_;
                            }
                        }
                        ++target_mask_size;
                    }
                    ++truth_idx;
                    ++target_idx;
                    break;
                }
                case insertion:
                {
                    if (in_flank) {
                        if (prev_state == insertion) {
                            result += get(gap_extend, truth_idx - 1) + nuc_prior;
                        } else {
                            // gap open score is charged for insertions just after the corresponding base, hence the -1
                            result += get(gap_open, truth_idx - 1) + nuc_prior;
                        }
                        ++target_mask_size;
                    }
                    ++target_idx;
                    break;
                }
                case deletion:
                {
                    if (in_flank) {
                        if (prev_state == deletion) {
                            result += get(gap_extend, truth_idx);
                        } else {
                            result += get(gap_open, truth_idx);
                        }
                    }
                    ++truth_idx;
                    break;
                }
            }
            ++alignment_idx;
            prev_state = new_state;
        }
        return result;
    }

public:
    constexpr static char gap_label = '-';

    constexpr static const char* name() noexcept { return name_; }
    constexpr static int band_size() noexcept { return band_size_; }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior) const noexcept
    {
        constexpr static NullType null {};
        return align_helper(truth, target, qualities, truth_len, target_len, gap_open, gap_extend, null, null, nuc_prior, null, null, null);
    }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const char* snv_mask,
          const std::int8_t* snv_prior,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior) const noexcept
    {
        constexpr static NullType null {};
        return align_helper(truth, target, qualities, truth_len, target_len, gap_open, gap_extend, snv_mask, snv_prior, nuc_prior, null, null, null);
    }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior,
          int& first_pos,
          char* align1,
          char* align2) const noexcept
    {
        constexpr static NullType null {};
        return align_helper(truth, target, qualities, truth_len, target_len, gap_open, gap_extend, null, null, nuc_prior, first_pos, align1, align2);
    }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    align(const char* truth,
          const char* target,
          const std::int8_t* qualities,
          const int truth_len,
          const int target_len,
          const char* snv_mask,
          const std::int8_t* snv_prior,
          const OpenPenaltyArrayOrConstant gap_open,
          const ExtendPenaltyArrayOrConstant gap_extend,
          short nuc_prior,
          int& first_pos,
          char* align1,
          char* align2) const noexcept
    {
        return align_helper(truth, target, qualities, truth_len, target_len, gap_open, gap_extend, snv_mask, snv_prior, nuc_prior, first_pos, align1, align2);
    }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    calculate_flank_score(const int truth_len,
                          const int lhs_flank_len,
                          const int rhs_flank_len,
                          const std::int8_t* quals,
                          const OpenPenaltyArrayOrConstant gap_open,
                          const ExtendPenaltyArrayOrConstant gap_extend,
                          const short nuc_prior,
                          const int first_pos,
                          const char* aln1,
                          const char* aln2,
                          int& target_mask_size) const noexcept
    {
        constexpr static NullType null {};
        return calculate_flank_score_helper(truth_len, lhs_flank_len, rhs_flank_len, null, quals, gap_open, gap_extend, null, null, nuc_prior, first_pos, aln1, aln2, target_mask_size);
    }

    template <typename OpenPenaltyArrayOrConstant,
              typename ExtendPenaltyArrayOrConstant>
    int
    calculate_flank_score(int truth_len,
                          int lhs_flank_len,
                          int rhs_flank_len,
                          const char* target,
                          const std::int8_t* quals,
                          const char* snv_mask,
                          const std::int8_t* snv_prior,
                          const OpenPenaltyArrayOrConstant gap_open,
                          const ExtendPenaltyArrayOrConstant gap_extend,
                          short nuc_prior,
                          int first_pos,
                          const char* aln1,
                          const char* aln2,
                          int& target_mask_size) const noexcept
    {
        return calculate_flank_score_helper(truth_len, lhs_flank_len, rhs_flank_len, target, quals, gap_open, gap_extend, snv_mask, snv_prior, nuc_prior, first_pos, aln1, aln2, target_mask_size);
    }
};

} // namespace simd
} // namespace hmm
} // namespace octopus

#endif


#include "coding/base64.hpp"

#include "3party/tomcrypt/src/headers/tomcrypt.h"
#include "3party/tomcrypt/src/headers/tomcrypt_misc.h"
#include "base/assert.hpp"

#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wreorder"
#include <boost/archive/iterators/base64_from_binary.hpp>
#include <boost/archive/iterators/binary_from_base64.hpp>
#include <boost/archive/iterators/transform_width.hpp>
#pragma GCC diagnostic pop

using namespace boost::archive::iterators;
typedef base64_from_binary<transform_width<string::const_iterator, 6, 8> > base64_t;
typedef transform_width<binary_from_base64<string::const_iterator>, 8, 6 > binary_t;

/// This namespace contains historically invalid implementation of base64,
/// but it's still needed for production code
namespace base64_for_user_ids
{
  string encode(string rawBytes)
  {
    // http://boost.2283326.n4.nabble.com/boost-archive-iterators-base64-from-binary-td2589980.html
    switch (rawBytes.size() % 3)
    {
    case 1:
      rawBytes.push_back('0');
    case 2:
      rawBytes.push_back('0');
      break;
    }

    return string(base64_t(rawBytes.begin()), base64_t(rawBytes.end()));
  }

  string decode(string const & base64Chars)
  {
    if (base64Chars.empty())
      return string();
    // minus 1 needed to avoid finishing zero in a string
    return string(binary_t(base64Chars.begin()), binary_t(base64Chars.begin() + base64Chars.size() - 1));
  }
}

namespace base64
{

string Encode(string const & bytesToEncode)
{
  string result;
  long unsigned int resSize = (bytesToEncode.size() + 3) * 4 / 3;
  // Raw new/delete because we don't throw exceptions inbetween
  unsigned char * buffer = new unsigned char[resSize];
  if (CRYPT_OK == base64_encode(reinterpret_cast<unsigned char const *>(bytesToEncode.data()),
                                bytesToEncode.size(),
                                buffer,
                                &resSize))
    result.assign(reinterpret_cast<char const *>(buffer), resSize);
  else
    ASSERT(false, ("It should work!"));

  delete[] buffer;
  return result;
}

string Decode(string const & base64CharsToDecode)
{
  string result;
  long unsigned int resSize = base64CharsToDecode.size() * 3 / 4 + 2;
  // Raw new/delete because we don't throw exceptions inbetween
  unsigned char * buffer = new unsigned char[resSize];
  if (CRYPT_OK == base64_decode(reinterpret_cast<unsigned char const *>(base64CharsToDecode.data()),
                                base64CharsToDecode.size(),
                                buffer,
                                &resSize))
    result.assign(reinterpret_cast<char const *>(buffer), resSize);
  else
    ASSERT(false, ("It should work!"));

  delete[] buffer;
  return result;
}

}


///////////////////////////////////////////////////////////////
//  Copyright 2011 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_

#ifdef _MSC_VER
#  define _SCL_SECURE_NO_WARNINGS
#endif

#include "test.hpp"
#include <boost/multiprecision/cpp_dec_float.hpp>

void proc_that_throws()
{
   throw std::runtime_error("");
}

int main()
{
   boost::multiprecision::cpp_dec_float_50 a1(1), a2(1), b(3), c(-2);

   BOOST_WARN(a1);
   BOOST_WARN(a1 == b);
   BOOST_CHECK(a1);
   BOOST_CHECK(a1 == b);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   boost::multiprecision::cpp_dec_float_50 a_tol = a1 + a1 * 100 * std::numeric_limits<boost::multiprecision::cpp_dec_float_50>::epsilon();
   boost::multiprecision::cpp_dec_float_50 tol = 100 * std::numeric_limits<boost::multiprecision::cpp_dec_float_50>::epsilon();

   BOOST_CHECK_CLOSE(a1, a_tol, tol * 102);  // Passes
   BOOST_WARN_CLOSE(a1, a_tol, tol * 98);    // fails
   BOOST_CHECK_CLOSE(a1, a_tol, tol * 98);    // fails
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_CLOSE_FRACTION(a1, a_tol, tol * 1.02);  // Passes
   BOOST_WARN_CLOSE_FRACTION(a1, a_tol, tol * 0.98);    // fails
   BOOST_CHECK_CLOSE_FRACTION(a1, a_tol, tol * 0.98);    // fails
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_EQUAL(a1, a2);
   BOOST_WARN_EQUAL(a1, b);
   BOOST_CHECK_EQUAL(a1, b);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_NE(a1, b);
   BOOST_WARN_NE(a1, a2);
   BOOST_CHECK_NE(a1, a2);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_GT(a1, c);
   BOOST_WARN_GT(a1, a2);
   BOOST_CHECK_GT(a1, a2);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_LT(a1, b);
   BOOST_WARN_LT(a1, a2);
   BOOST_CHECK_LT(a1, a2);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_GE(a1, a2);
   BOOST_CHECK_GE(a1, c);
   BOOST_WARN_GE(a1, b);
   BOOST_CHECK_GE(a1, b);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   BOOST_CHECK_LE(a1, a2);
   BOOST_CHECK_LE(a1, b);
   BOOST_WARN_LE(a1, c);
   BOOST_CHECK_LE(a1, c);
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

#ifndef BOOST_NO_EXCEPTIONS
   BOOST_CHECK_THROW(proc_that_throws(), std::runtime_error);
   BOOST_CHECK_THROW(a1+b+c, std::runtime_error);
#endif
   BOOST_CHECK(boost::detail::test_errors() == 1);
   --boost::detail::test_errors();

   return boost::report_errors();
}


//==================================================================================================
/*!
  @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_ISINCLUDED_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_ISINCLUDED_HPP_INCLUDED

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{
  /*!
    @ingroup group-reduction
    This function object returns @c true is only if all bits set in @c x are
  also set in @c y

    The two arguments must share the same bit size.

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

    @par Example:

      @snippet isincluded.cpp isincluded

    @par Possible output:

      @snippet isincluded.txt isincluded

  **/
  bool isincluded(Value const& x, Value const& y);
} }
#endif

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

#endif


//------------------------------------------------------------------------------
/// \file
/// \ingroup meshing
/// \copyright (C) Copyright Aquaveo 2018. Distributed under FreeBSD License
/// (See accompanying file LICENSE or https://aqaveo.com/bsd/license.txt)
//------------------------------------------------------------------------------

//----- Included files ---------------------------------------------------------

// 1. Precompiled header

// 2. My own header
#include <xmsmesh/meshing/MePolyRedistributePts.h>

// 3. Standard library headers
#include <cfloat>
#include <sstream>

// 4. External library headers
#include <boost/make_shared.hpp>
#include <boost/thread/mutex.hpp>

// 5. Shared code headers
#include <xmscore/points/pt.h>
#include <xmscore/math/math.h>
#include <xmscore/misc/StringUtil.h>
#include <xmscore/stl/vector.h>
#include <xmsinterp/geometry/GmMultiPolyIntersector.h>
#include <xmsinterp/geometry/GmMultiPolyIntersectionSorter.h>
#include <xmsinterp/geometry/GmMultiPolyIntersectionSorterTerse.h>
#include <xmsinterp/interpolate/InterpBase.h>
#include <xmsmesh/meshing/detail/MePolyOffsetter.h>
#include <xmsmesh/meshing/detail/MePolyRedistributePtsCurvature.h>
#include <xmscore/misc/xmstype.h>
#include <xmscore/misc/XmError.h>
#include <xmscore/misc/XmLog.h>
#include <xmsinterp/triangulate/TrTriangulatorPoints.h>
#include <xmscore/misc/XmConst.h>

// 6. Non-shared code headers

//----- Forward declarations ---------------------------------------------------

//----- External globals -------------------------------------------------------

//----- Namespace declaration --------------------------------------------------
namespace xms
{
//----- Constants / Enumerations -----------------------------------------------

//----- Classes / Structs ------------------------------------------------------

//----- Internal functions -----------------------------------------------------

//----- Class / Function definitions -------------------------------------------

class MePolyRedistributePtsImpl : public MePolyRedistributePts
{
public:
  MePolyRedistributePtsImpl()
  : m_constSize(XM_NONE)
  , m_minLength(XM_DBL_HIGHEST)
  , m_maxLength(XM_DBL_LOWEST) // changed to be XM_DBL_LOWEST since it is an arbitrarily low value
  , m_sizeBias(1.0)
  , m_polyPts(new VecPt3d())
  , m_intersectWithTris(false)
  , m_distSqTol(0)
  , m_biasConstSize(false)
  , m_polyOffsetIter(0)
  , m_featureSizeCurvature(0)
  , m_meanSpacingCurvature(0)
  , m_minimumCurvature(0.001)
  , m_smoothCurvature(false)
  , m_curvatureRedist()
  {
  }

  virtual void SetSizeFunc(BSHP<InterpBase> a_interp) override;
  virtual void SetSizeFuncFromPoly(const VecPt3d& a_outPoly,
                                   const VecPt3d2d& a_inPolys,
                                   double a_sizeBias) override;

  //------------------------------------------------------------------------------
  /// \brief Sets the size function to a constant value
  /// \param a_size The element edge size.
  //------------------------------------------------------------------------------
  virtual void SetConstantSizeFunc(double a_size) override
  {
    m_constSize = a_size;
    m_curvatureRedist.reset(); // remove curvature redistribution
  }                            // SetConstantSizeFunc

  //------------------------------------------------------------------------------
  /// \brief Sets the bias for constant value size function
  /// \param a_sizeBias Transition rate for size function
  //------------------------------------------------------------------------------
  virtual void SetConstantSizeBias(double a_sizeBias) override
  {
    m_sizeBias = a_sizeBias;
    m_biasConstSize = true;
    m_curvatureRedist.reset(); // remove curvature redistribution
  }                            // SetConstantSizeBias

  void SetUseCurvatureRedistribution(double a_featureSize,
                                     double a_meanSpacing,
                                     double a_minimumCurvature,
                                     bool a_smooth) override;

  void Redistribute(const MePolyOffsetterOutput& a_input,
                    MePolyOffsetterOutput& a_out,
                    int a_polyOffsetIter);
  virtual VecPt3d Redistribute(const VecPt3d& a_polyLine) override;
  virtual double SizeFromLocation(const Pt3d& a_location) override;
  virtual std::string ToPyRepr() const override;

  VecPt3d LoopToVecPt3d(const VecSizet& a_idx, const VecPt3d& a_pts);
  void IntersectWithTris(VecPt3d& a_pts);
  void InterpEdgeLengths(const VecPt3d& a_pts, VecDbl& lengths);
  void InterpToPoint(size_t a_idx,
                     const VecPt3d& a_pts,
                     VecDbl& a_lengths,
                     VecDbl& a_wt,
                     VecDbl& a_d2,
                     double& a_bias);
  void CalcInterpWeights(const Pt3d& a_pt, VecDbl& a_wt, VecDbl& a_d2, double& a_sumWt);
  VecPt3d RedistPts(const VecPt3d& a_pts, const VecDbl& lengths);
  VecPt3d RedistPts2(const VecPt3d& a_pts, const VecDbl& lengths);
  void RedistPtsToOutput(const VecPt3d& a_pts, int a_polyType, MePolyOffsetterOutput& a_out);

  void TvaluesForSeg(size_t a_idx,
                     size_t a_nextIdx,
                     const VecDbl& a_segLengths,
                     const VecDbl& a_segTvalues,
                     const VecDbl& a_interpLengths,
                     double& a_pcntin,
                     VecDbl& a_tVals);
  void SizeFromPolyCalcAveEdgeLengthAtPts(const VecPt3d& a_outPoly, const VecPt3d2d& a_inPolys);
  void SizeFromPolyAddEdgeLengths(const VecPt3d& a_pts);
  void CalcSegLengths(const VecPt3d& a_pts, VecDbl& a_segLength, VecDbl& _segTvalues);
  void GetSegmentFromTval(const VecDbl& a_segTvalues,
                          double a_tVal,
                          size_t& a_segIdx,
                          double& a_segT0,
                          double& a_segT1);
  void CreatePolyIntersector();

  double m_constSize,        ///< constant size function
    m_minLength,             ///< min segment length in polygon
    m_maxLength,             ///< max segment length in polygon
    m_sizeBias;              ///< transition factor for size function
  BSHP<VecPt3d> m_polyPts;   ///< polygon point locations
  VecDbl m_polyEdgeLengths;  ///< edge lengths of the polygon
  BSHP<InterpBase> m_interp; ///< interpolation class given to this class
  // BSHP<InterpIdw>              m_idw; ///< interpolation class used with larger numbers of
  // polygon points
  BSHP<VecPt3d> m_sizePts;                        ///< points from the m_interp class
  BSHP<VecInt> m_sizeTris;                        ///< triangles from the m_interp class
  BSHP<GmMultiPolyIntersector> m_polyIntersector; ///< used to intersect the polygon with m_sizeTris
  VecInt2d m_polys; ///< polygon definition of triangles for m_polyIntersector
  /// flag to indicate that polygon should be intersected with the triangles
  bool   m_intersectWithTris;
  double m_distSqTol;    ///< tolerance used to speed up interpolation
  bool m_biasConstSize;  ///< flag to indicate transitioning to constant size function
  int m_polyOffsetIter;  ///< number of iterations from the polygon boundary
  /// Used by curvature redistribution. The size of the smallest feature in the polyline to be
  /// detected. Large values will generate point distributions that follow coarser curvatures.
  double m_featureSizeCurvature;
  /// Used by curvature redistribution. The mean spacing between the distributed points.
  double m_meanSpacingCurvature;
  /// Used by curvature redistribution. The value of the curvature to be used instead of 0 in
  /// staight lines. It limits the maximum spacing between points. If not included, the default
  /// is 0.001.
  double m_minimumCurvature;
  /// Used by curvature redistribution. Detemines if the curvatures are to be averaged by a
  /// rolling 0.25-0.5-0.25 weighted rolling average.
  bool m_smoothCurvature;
  /// Point redistributor that uses curvature
  BSHP<MePolyRedistributePtsCurvature> m_curvatureRedist;
};

//------------------------------------------------------------------------------
/// \brief Creates an instance of this class
/// \return MePolyRedistributePts
//------------------------------------------------------------------------------
BSHP<MePolyRedistributePts> MePolyRedistributePts::New()
{
  BSHP<MePolyRedistributePts> ret(new MePolyRedistributePtsImpl);
  return ret;
} // MePolyRedistributePts::New
//------------------------------------------------------------------------------
/// \brief destructor
//------------------------------------------------------------------------------
MePolyRedistributePts::~MePolyRedistributePts()
{
} // MePolyRedistributePts::~MePolyRedistributePts
//------------------------------------------------------------------------------
/// \brief constructor
//------------------------------------------------------------------------------
MePolyRedistributePts::MePolyRedistributePts()
{
} // MePolyRedistributePts::MePolyRedistributePts

////////////////////////////////////////////////////////////////////////////////
/// \class MePolyRedistributePtsImpl
/// \brief Redistributes points along polylines using a size function.
////////////////////////////////////////////////////////////////////////////////
//------------------------------------------------------------------------------
/// \brief Sets the size function interpolator
/// \param a_interp Size function interpolator class
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::SetSizeFunc(BSHP<InterpBase> a_interp)
{
  m_curvatureRedist.reset(); // remove curvature redistribution
  m_interp = a_interp;
  m_intersectWithTris = true;
  // make sure we have triangles
  m_sizePts = m_interp->GetPts();
  m_sizeTris = m_interp->GetTris();
  if (m_intersectWithTris && m_sizeTris->empty())
  {
    VecInt vTris;
    TrTriangulatorPoints tri(*m_sizePts, vTris);
    tri.Triangulate();
    if (!vTris.empty())
    {
      m_sizeTris = BSHP<VecInt>(new VecInt());
      m_sizeTris->reserve(vTris.size());
      for (size_t i = 0; i < vTris.size(); ++i)
        m_sizeTris->push_back((int)vTris[i]);
    }
  }
  if (m_intersectWithTris)
    CreatePolyIntersector();
} // SetSizeFunc
//------------------------------------------------------------------------------
/// \brief Creates an interpolator that uses the spacing on the input polygon
/// as its scalar
/// \param a_outPoly The outside polygon
/// \param a_inPolys Inside polygons that are inside of a_outPoly
/// \param a_sizeBias A factor used in transitioning the size
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::SetSizeFuncFromPoly(const VecPt3d& a_outPoly,
                                                    const VecPt3d2d& a_inPolys,
                                                    double a_sizeBias)
{
  m_curvatureRedist.reset(); // remove curvature redistribution
  m_sizeBias = a_sizeBias;
  // calculate the lengths of the segments
  SizeFromPolyCalcAveEdgeLengthAtPts(a_outPoly, a_inPolys);
  if (m_polyPts->empty())
    return;
  double diff = m_maxLength - m_minLength;
  if (diff / m_minLength < .05)
  {
    m_constSize = (m_maxLength + m_minLength) / 2;
  }
  // looking for a way to speed up interpolation of edge lengths
  // doesn't work well
  // else if (m_polyPts->size() > 100)
  //{
  //  m_idw = InterpIdw::New();
  //  m_idw->SetPtsTris(m_polyPts, m_sizeTris);
  //  BSHP<VecFlt> s(new VecFlt(m_polyEdgeLengths.size()));
  //  for (size_t i=0; i<m_polyEdgeLengths.size(); ++i)
  //    (*s)[i] = (float)m_polyEdgeLengths[i];
  //  m_idw->SetScalars(s);
  //  m_idw->SetSearchOpts(32, false);
  //  m_idw->SetWeightCalcMethod(InterpIdw::CLASSIC);
  //}
  else
  {
    size_t nPts = m_polyPts->size();
    Pt3d* p(&(*m_polyPts)[0]);
    Pt3d pMin(p[0]), pMax(p[0]);
    for (size_t i = 0; i < nPts; ++i)
    {
      if (p[i].x < pMin.x)
        pMin.x = p[i].x;
      if (p[i].y < pMin.y)
        pMin.y = p[i].y;
      if (p[i].x > pMax.x)
        pMax.x = p[i].x;
      if (p[i].y > pMax.y)
        pMax.y = p[i].y;
    }
    // compute distance to avoid interpolating too much
    double boundsDistSq = MdistSq(pMin.x, pMin.y, pMax.x, pMax.y);
    m_distSqTol = 1e-4 * boundsDistSq;
  }
} // MePolyRedistributePtsImpl::SetSizeFuncFromPoly
//------------------------------------------------------------------------------
/// \brief Specifies that curvature redistribution will be used
/// \param[in] a_featureSize: The size of the smallest feature in the polyline to be detected.
///   Large values will generate point distributions that follow coarser curvatures.
/// \param[in] a_meanSpacing: The mean spacing between the distributed points.
/// \param[in] a_minimumCurvature: The value of the curvature to be used instead of 0 in staight
///   lines. It limits the maximum spacing between points. If not included, the default is 0.001.
/// \param[in] a_smooth: Detemines if the curvatures are to be averaged by a rolling 0.25-0.5-0.25
///   weighted rolling average.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::SetUseCurvatureRedistribution(double a_featureSize,
                                                              double a_meanSpacing,
                                                              double a_minimumCurvature,
                                                              bool a_smooth)
{
  m_featureSizeCurvature = a_featureSize;
  m_meanSpacingCurvature = a_meanSpacing;
  m_minimumCurvature = a_minimumCurvature;
  m_smoothCurvature = a_smooth;
  m_curvatureRedist = MePolyRedistributePtsCurvature::New();
} // MePolyRedistributePtsImpl::SetUseCurvatureRedistribution
//------------------------------------------------------------------------------
/// \brief Redistributes points on closed loop polylines. The length of edges
/// in the redistribution comes from a size function that is interpolated to
/// the points that make up the polylines. By default this size function comes
/// from the edge lengths in the original polygon.
/// \param a_input Input closed loop polylines
/// \param a_out Redistributed closed loop polylines
/// \param a_polyOffsetIter Number of iterations from the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::Redistribute(const MePolyOffsetterOutput& a_input,
                                             MePolyOffsetterOutput& a_out,
                                             int a_polyOffsetIter)
{
  VecDbl lengths;
  m_polyOffsetIter = a_polyOffsetIter;
  a_out.m_loops.resize(0);
  a_out.m_pts.resize(0);
  a_out.m_loopTypes.resize(0);
  for (size_t i = 0; i < a_input.m_loops.size(); ++i)
  {
    const VecSizet& loop(a_input.m_loops[i]);
    const int& lType(a_input.m_loopTypes[i]);
    VecPt3d pts = LoopToVecPt3d(loop, a_input.m_pts);
    if (m_intersectWithTris)
    {
      IntersectWithTris(pts);
    }
    pts.push_back(pts.front());
    // interpolate edge lengths
    InterpEdgeLengths(pts, lengths);
    // redistribute the points
    VecPt3d redistPts = RedistPts(pts, lengths);
    if (!redistPts.empty())
      redistPts.pop_back();
    RedistPtsToOutput(redistPts, lType, a_out);
  }
  m_polyOffsetIter = 0;
} // MePolyPaverToMeshPtsImpl::Redistribute
//------------------------------------------------------------------------------
/// \brief Redistributes points on a polyline.
/// \param[in] a_polyLine Input polyline
/// \return Redistributed polyline
//------------------------------------------------------------------------------
VecPt3d MePolyRedistributePtsImpl::Redistribute(const VecPt3d& a_polyLine)
{
  if (m_curvatureRedist)
  {
    return m_curvatureRedist->Redistribute(a_polyLine, m_featureSizeCurvature,
                                           m_meanSpacingCurvature, m_minimumCurvature,
                                           m_smoothCurvature);
  }
  VecPt3d pts(a_polyLine), ret;
  VecDbl lengths;
  if (m_intersectWithTris)
  {
    IntersectWithTris(pts);
  }
  // interpolate edge lengths
  InterpEdgeLengths(pts, lengths);
  // redistribute the points
  ret = RedistPts(pts, lengths);
  return ret;
} // MePolyRedistributePtsImpl::Redistribute
//------------------------------------------------------------------------------
/// \brief returns a size based on the xy location
/// \param[in] a_location: the location
/// \return size function value for the desired edge length at the location
//------------------------------------------------------------------------------
double MePolyRedistributePtsImpl::SizeFromLocation(const Pt3d& a_location)
{
  if (m_curvatureRedist)
  {
    std::string msg = "MePolyRedistributePts set to use curvature redistribution; "
      "MePolyRedistributePtsImpl::SizeFromLocation can not be call with these "
      "settings. XM_NODATA will be returned.";
    boost::mutex mtx;
    mtx.lock();
    XM_LOG(xmlog::error, msg);
    mtx.unlock();
    return XM_NODATA;
  }
  VecPt3d pts(1, a_location);
  VecDbl lengths;
  InterpEdgeLengths(pts, lengths);
  return lengths.front();
} // MePolyRedistributePtsImpl::SizeFromLocation
//------------------------------------------------------------------------------
/// \brief returns a string for use in the python __repr__ attribute
/// \return a __repr__ string
//------------------------------------------------------------------------------
std::string MePolyRedistributePtsImpl::ToPyRepr() const
{
  std::stringstream ss;
  ss << "size_bias: " << STRstd(m_sizeBias) << "\n";
  if (m_constSize != XM_NONE)
    ss << "constant_size_function: " << STRstd(m_constSize) << "\n";
  if (m_interp)
    ss << "size_func: <class: InterpBase>\n";
  if (m_featureSizeCurvature != 0)
  {
    ss << "feature_size_curvature: " << STRstd(m_featureSizeCurvature) << "\n";
    ss << "mean_spacing_curvature: " << STRstd(m_meanSpacingCurvature) << "\n";
    ss << "minimum_curvature: " << STRstd(m_minimumCurvature) << "\n";
    std::string offOn[2] = {"False", "True"};
    ss << "smooth_curvature: " << offOn[(int)m_smoothCurvature] << "\n";
  }
  return ss.str();
} // MePolyRedistributePtsImpl::ToPyRepr
//------------------------------------------------------------------------------
/// \brief Creates a vector of pts from indices into another vector of points
/// \param a_idx Vector of indices into the a_pts vector. Defines the closed
/// loop polyline.
/// \param a_pts Vector of locations.
/// \return Vector of locations defining the loop.
//------------------------------------------------------------------------------
VecPt3d MePolyRedistributePtsImpl::LoopToVecPt3d(const VecSizet& a_idx, const VecPt3d& a_pts)
{
  VecPt3d ret;
  ret.reserve(a_idx.size());
  for (size_t i = 0; i < a_idx.size(); ++i)
    ret.push_back(a_pts[a_idx[i]]);
  return ret;
} // MePolyRedistributePtsImpl::LoopToVecPt3d
//------------------------------------------------------------------------------
/// \brief Creates a vector of pts from indices into another vector of points
/// \param a_pts Vector of locations.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::IntersectWithTris(VecPt3d& a_pts)
{
  VecPt3d newPts, pts;
  VecInt polys;
  for (size_t i = 0; i < a_pts.size(); ++i)
  {
    Pt3d p0(a_pts[i]), p1(a_pts[0]);
    if (i < a_pts.size() - 1)
      p1 = a_pts[i + 1];
    m_polyIntersector->TraverseLineSegment(p0.x, p0.y, p1.x, p1.y, polys, pts);
    auto start = pts.begin();
    if (i > 0)
      ++start;
    newPts.insert(newPts.end(), start, pts.end());
  }
  a_pts.swap(newPts);
  a_pts.pop_back();
} // MePolyRedistributePtsImpl::IntersectWithTris
//------------------------------------------------------------------------------
/// \brief Interpolates edge lengths to each of the points that make up a
/// polyline
/// \param a_pts Vector of locations.
/// \param a_lengths Vector of interpolated lengths filled in by the method.
/// Will be the same size as a_idx.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::InterpEdgeLengths(const VecPt3d& a_pts, VecDbl& a_lengths)
{
  a_lengths.assign(a_pts.size(), 0.0);
  if (XM_NONE != m_constSize && !m_biasConstSize)
  {
    a_lengths.assign(a_pts.size(), m_constSize);
    return;
  }
  double bias(XM_NONE);
  if (XM_NONE != m_constSize && m_biasConstSize)
  {
    bias = m_sizeBias;
    if (bias > .99)
      bias = .99;
    else if (bias < .01)
      bias = .01;
    bias = 1 - bias;
    bias = pow(bias, (double)m_polyOffsetIter);
  }

  if (m_interp)
  { // size function interpolation
    VecFlt s;
    m_interp->InterpToPts(a_pts, s);
    for (size_t i = 0; i < s.size(); ++i)
      a_lengths[i] = (double)s[i];
  }
  // tried to use to speed up but was not successful
  // else if (m_idw)
  //{
  //  VecFlt s;
  //  m_idw->InterpToPts(a_pts, s);
  //  for (size_t i=0; i<s.size(); ++i) a_lengths[i] = (double)s[i];
  //}
  else if (!m_polyEdgeLengths.empty())
  {
    double lastInterp(-1), distSq(0);
    VecDbl wt, d2;
    Pt3d aPt;
    InterpToPoint(0, a_pts, a_lengths, wt, d2, bias);
    lastInterp = a_lengths[0];
    aPt = a_pts[0];

    // do the rest of the points
    for (size_t i = 1; i < a_pts.size(); ++i)
    {
      distSq = MdistSq(aPt.x, aPt.y, a_pts[i].x, a_pts[i].y);
      if (distSq < m_distSqTol)
      {
        a_lengths[i] = lastInterp;
        continue;
      }
      InterpToPoint(i, a_pts, a_lengths, wt, d2, bias);
      lastInterp = a_lengths[i];
      aPt = a_pts[i];
    }
  }
  else
  {
    boost::mutex mtx;
    mtx.lock();
    VecDbl lengths, tvals;
    CalcSegLengths(a_pts, lengths, tvals);
    double sum(0);
    for (size_t i = 0; i < lengths.size(); ++i)
      sum += lengths[i];
    sum = sum / lengths.size();
    m_constSize = sum;
    std::stringstream ss;
    ss << "Interpolator not defined in MePolyRedistributePts. Size function "
          "set to constant value: "
       << sum << ".";
    XM_LOG(xmlog::debug, ss.str());
    InterpEdgeLengths(a_pts, a_lengths);
    mtx.unlock();
  }
} // MePolyRedistributePtsImpl::InterpEdgeLengths
//------------------------------------------------------------------------------
/// \brief Interpolates to a point from the boundary of the polygon
/// \param[in] a_idx Index of point
/// \param[in] a_pts Vector of point locations
/// \param[out] a_lengths Vector of lengths. Value at a_idx computed.
/// \param[out] a_wt Vector of weights from polygon points filled in
/// CalcInterpWeights.
/// \param[out] a_d2 Vector of distance squared between polygon points and the
/// point at a_idx that is filled by CalcInterpWeights.
/// \param[in] a_bias A factor for scaling the length when doing a smooth
/// transition to a constant size function.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::InterpToPoint(size_t a_idx,
                                              const VecPt3d& a_pts,
                                              VecDbl& a_lengths,
                                              VecDbl& a_wt,
                                              VecDbl& a_d2,
                                              double& a_bias)
{
  double sumWt(0);
  CalcInterpWeights(a_pts[a_idx], a_wt, a_d2, sumWt);
  for (size_t j = 0; j < m_polyEdgeLengths.size(); ++j)
  {
    a_lengths[a_idx] += m_polyEdgeLengths[j] * (a_wt[j] / sumWt);
  }
  if (XM_NONE != a_bias)
  {
    double& d(a_lengths[a_idx]);
    if (d > m_constSize)
    {
      d = d * a_bias;
      if (d < m_constSize)
        d = m_constSize;
    }
    else
    {
      d = d / a_bias;
      if (d > m_constSize)
        d = m_constSize;
    }
  }
  else
  {
    if (a_lengths[a_idx] < m_minLength)
      a_lengths[a_idx] = m_minLength;
    if (a_lengths[a_idx] > m_maxLength)
      a_lengths[a_idx] = m_maxLength;
  }
} // MePolyRedistributePtsImpl::InterpToPoint
//------------------------------------------------------------------------------
/// \brief Calculates the interpolation weights of the points used to do the
/// interpolation
/// \param a_pt The location being interpolated to.
/// \param a_wt Vector of weights calculated by this function
/// \param a_d2 Vector of distance squared
/// \param a_sumWt Sum of the weights in a_wt
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::CalcInterpWeights(const Pt3d& a_pt,
                                                  VecDbl& a_wt,
                                                  VecDbl& a_d2,
                                                  double& a_sumWt)
{
  a_sumWt = 0;
  double diff(0), factor(0);
  size_t nPts = m_polyPts->size();
  const Pt3d* p(&(*m_polyPts)[0]);
  a_wt.resize(nPts);
  a_d2.resize(nPts);
  for (size_t i = 0; i < nPts; ++i)
  {
    a_d2[i] = MdistSq(a_pt.x, a_pt.y, p[i].x, p[i].y);
    if (a_d2[i] < 10e-8)
      a_wt[i] = 10e10;
    else
      a_wt[i] = 1 / a_d2[i];

    diff = m_polyEdgeLengths[i] - m_minLength;
    factor = m_minLength + (m_sizeBias * diff);
    a_wt[i] *= factor;

    a_sumWt += a_wt[i];
  }
} // MePolyRedistributePtsImpl::CalcInterpWeights
//------------------------------------------------------------------------------
/// \brief Uses interpolated lengths to redistribute points on a polyline
/// \param a_pts Vector of locations.
/// \param a_interpLengths Vector of interpolated lengths.
/// \return Vector of locations of redistributed points
//------------------------------------------------------------------------------
VecPt3d MePolyRedistributePtsImpl::RedistPts(const VecPt3d& a_pts, const VecDbl& a_interpLengths)
{
  static bool debug(false);
  if (debug)
    return RedistPts2(a_pts, a_interpLengths);

  // Get lengths of all segments and the total length
  VecDbl segLength, segTvalues;
  CalcSegLengths(a_pts, segLength, segTvalues);

  // create array of T values for locations of new points along this loop
  double pcntin(0);
  size_t nextIdx;
  VecDbl tVals;
  tVals.reserve(a_pts.size());
  tVals.push_back(0.0); // this is the first and last point in our loop
  for (size_t i = 0; i < a_pts.size() - 1; ++i)
  {
    nextIdx = 0;
    if (i + 1 < a_pts.size() - 1)
      nextIdx = i + 1;
    TvaluesForSeg(i, nextIdx, segLength, segTvalues, a_interpLengths, pcntin, tVals);
  }
  double aveTincrement(0);
  for (size_t i = 1; i < tVals.size(); ++i)
  {
    aveTincrement += tVals[i] - tVals[i - 1];
  }
  aveTincrement /= (tVals.size() - 1);

  VecPt3d ret;
  // adjust the tvals based on what was left over when processing the last
  // segment. If the left over tval is less than .5 of the last segment then
  // we don't keep that last tval
  double leftOverTval = 1 - tVals.back();
  if (tVals.size() < 2)
    return ret;
  if (pcntin < .5)
  {
    tVals.pop_back();
    if (leftOverTval >= 0)
      leftOverTval = 1 - tVals.back();
  }
  if (tVals.size() > 2)
  { // redistribute the leftover tvalue to all the tVals
    double redistT = (leftOverTval - aveTincrement) / tVals.size();
    for (size_t i = 1; i < tVals.size(); ++i)
      tVals[i] += (i * redistT);
  }
  else if (segLength.size() > 3)
  { // we initially had at least 3 points
    tVals.assign(3, 0);
    tVals[1] = 1 / 3.0;
    tVals[2] = 2 / 3.0;
  }

  if (tVals.size() > 2)
  {
    tVals.push_back(1.0);
    ret.reserve(tVals.size());
    // create the points
    // put in the first point
    ret.push_back(a_pts[0]);
    // set the z to 0.0
    ret[0].z = 0;
    size_t segIdx(0);
    for (size_t i = 1; i < tVals.size(); ++i)
    { // get the segment that the tval is on
      double segT0, segT1, tVal = tVals[i];
      GetSegmentFromTval(segTvalues, tVals[i], segIdx, segT0, segT1);
      // convert polyline tval to segment tval
      tVal = (tVal - segT0) / (segT1 - segT0);
      // calculate the point
      size_t nextIdx = 0;
      if (segIdx + 1 < a_pts.size())
        nextIdx = segIdx + 1;
      const Pt3d &p0(a_pts[segIdx]), &p1(a_pts[nextIdx]);
      Pt3d pt;
      pt.x = p0.x + tVal * (p1.x - p0.x);
      pt.y = p0.y + tVal * (p1.y - p0.y);
      ret.push_back(pt);
    }
  }
  return ret;
} // MePolyRedistributePtsImpl::RedistPts
//------------------------------------------------------------------------------
/// \brief Uses interpolated lengths to redistribute points on a polyline
/// \param a_pts Vector of locations.
/// \param a_interpLengths Vector of interpolated lengths.
/// \return Vector of locations of redistributed points
//------------------------------------------------------------------------------
VecPt3d MePolyRedistributePtsImpl::RedistPts2(const VecPt3d& a_pts, const VecDbl& a_interpLengths)
{
  VecPt4d finalpts;
  finalpts.reserve(a_pts.size());
  int numint((int)a_pts.size());
  double pcntin(0), pcntleft(0);
  int nextid(1);
  Pt3d prevpt(a_pts[0]);
  prevpt.z = a_interpLengths[0];
  Pt3d nextpt(a_pts[1]);
  nextpt.z = a_interpLengths[1];
  double len_segs(0), len_arc(0), length(0);
  len_arc = length = Mdist(prevpt.x, prevpt.y, nextpt.x, nextpt.y);
  int numfinal(0);
  double totdist(0), w1(0), w2(0);
  bool done(false);
  while (!done)
  {
    w1 = prevpt.z; // weight at first point
    w2 = nextpt.z; // weight at second point
    pcntleft = pcntin + 2 * length / (w1 + w2);
    // check if we should add a point
    if (pcntleft >= 1)
    { // calc new pt location
      double scale = 1.0 - pcntin;
      double ptlength = 2 * scale * w1 * length / (2 * length + scale * (w1 - w2));
      totdist += ptlength;
      // weight at new point
      double pcnt = ptlength / length;
      double newweight = w1 * (1 - pcnt) + w2 * pcnt;
      // add to final points
      ++numfinal;
      finalpts.push_back(Pt4d());
      finalpts[numfinal - 1].x = prevpt.x + pcnt * (nextpt.x - prevpt.x);
      finalpts[numfinal - 1].y = prevpt.y + pcnt * (nextpt.y - prevpt.y);
      finalpts[numfinal - 1].z = newweight;
      finalpts[numfinal - 1].w = totdist;
      len_segs += totdist;
      prevpt = finalpts[numfinal - 1];
      totdist = 0.0;
      // get length of segment left after inserting pt
      length -= ptlength;
      pcntin = 0.0;
    }
    else
    { // go on to next segment
      pcntin = pcntleft;
      if (++nextid < numint)
      {
        prevpt = nextpt;

        nextpt = a_pts[nextid];
        nextpt.z = a_interpLengths[nextid];
        totdist += length;
        length = Mdist(prevpt.x, prevpt.y, nextpt.x, nextpt.y);
        len_arc += length;
      }
      else
        done = true;
    }
  }

  // make sure we still have an arc
  if (numfinal > 0)
  {
    double len_segs_to_adjust = 0.0;
    // init length of the arc to adjust to remaining portion
    double len_arc_to_adjust = len_arc - len_segs;
    // init first point to move as the last in the list
    int first_pt_to_move = numfinal - 1;
    // eliminate last part_seg if less than half
    if (pcntin < 0.5)
      --numfinal;
    // keep the length of the last seg if more than half
    else
      len_segs_to_adjust = (length + totdist) / pcntleft;
    // find segs with length close to final length to find first pt to move
    while (first_pt_to_move >= 0 && finalpts[first_pt_to_move].w < w1 * 2.0 &&
           finalpts[first_pt_to_move].w > w1 * 0.5)
    {
      len_segs_to_adjust += finalpts[first_pt_to_move].w;
      len_arc_to_adjust += finalpts[first_pt_to_move].w;
      --first_pt_to_move;
    }
    ++first_pt_to_move;
    // find error adjustment scale
    double scale = len_arc_to_adjust / len_segs_to_adjust;
    /* reset the locations using scaled distance */
    nextid = 1;
    prevpt = a_pts[0];
    prevpt.z = a_interpLengths[0];
    nextpt = a_pts[1];
    nextpt.z = a_interpLengths[1];
    length = Mdist(prevpt.x, prevpt.y, nextpt.x, nextpt.y);
    for (int i = 0; i < numfinal; i++)
    {
      if (i < first_pt_to_move)
      {
        totdist = finalpts[i].w;
        // move along int points until reach finalpt
        while (totdist > length)
        {
          totdist -= length;
          // go onto next segment
          prevpt = nextpt;
          ++nextid;
          nextpt = a_pts[nextid];
          nextpt.z = a_interpLengths[nextid];
          length = Mdist(prevpt.x, prevpt.y, nextpt.x, nextpt.y);
        }
      }
      else
      {
        totdist = finalpts[i].w * scale;
        // move along int points until reach finalpt
        while (totdist > length)
        {
          totdist -= length;
          // go onto next segment
          prevpt = nextpt;
          ++nextid;
          nextpt = a_pts[nextid];
          nextpt.z = a_interpLengths[nextid];
          length = Mdist(prevpt.x, prevpt.y, nextpt.x, nextpt.y);
        }
        // reached point to move
        double pcnt = totdist / length;
        finalpts[i].x = prevpt.x + pcnt * (nextpt.x - prevpt.x);
        finalpts[i].y = prevpt.y + pcnt * (nextpt.y - prevpt.y);
      }
      prevpt = finalpts[i];
      length -= totdist;
    }
  }

  VecPt3d ret;
  // if arc closes, make sure there are at least 3 points on arc
  if (a_pts.front() == a_pts.back() && numfinal < 2)
  {
    if (a_pts.size() < 3)
      return ret;

    finalpts.resize(2);
    finalpts[0] = a_pts[a_pts.size() / 3];
    finalpts[1] = a_pts[a_pts.size() * 2 / 3];
  }
  ret.assign(finalpts.size() + 2, Pt3d());
  ret.front() = a_pts.front();
  ret.back() = a_pts.back();
  for (size_t i = 1; i < finalpts.size(); ++i)
    ret[i] = finalpts[i];
  return ret;
} // MePolyRedistributePtsImpl::RedistPts2
//------------------------------------------------------------------------------
/// \brief Move the redistributed points to the output class
/// \param a_pts Vector of locations of the redistributed points.
/// \param a_polyType The type of polygon that is represented by a_idx
/// (MePolyOffsetter::polytype)
/// \param a_out The output filled by this method. Creates a new polygon with
/// redistributed points.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::RedistPtsToOutput(const VecPt3d& a_pts,
                                                  int a_polyType,
                                                  MePolyOffsetterOutput& a_out)
{
  if (a_pts.empty())
    return;
  a_out.m_loopTypes.push_back(a_polyType);
  a_out.m_loops.push_back(VecSizet());
  for (size_t i = 0; i < a_pts.size(); ++i)
  {
    a_out.m_loops.back().push_back(a_out.m_pts.size());
    a_out.m_pts.push_back(a_pts[i]);
  }
} // MePolyRedistributePtsImpl::RedistPtsToOutput
//------------------------------------------------------------------------------
/// \brief A single segment of a polygon is analyzed to see where points are
/// redistributed on that segment.
/// \param a_idx Index of the segment
/// \param a_nextIdx The next index in the array. Since this is a closed loop
/// this value can wrap back to 0
/// \param a_segLengths Vector of segment lengths
/// \param a_segTvalues Vector of parametric values for the end points of
/// segments relative to the total polyline length
/// \param a_interpLengths Vector of interpolated edgelengths at the end points
/// of the segments
/// \param a_pcntin A running percentage that is added to by this method.
/// \param a_tVals Vector of parametric values for redistributed points. Filled
/// by this method.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::TvaluesForSeg(size_t a_idx,
                                              size_t a_nextIdx,
                                              const VecDbl& a_segLengths,
                                              const VecDbl& a_segTvalues,
                                              const VecDbl& a_interpLengths,
                                              double& a_pcntin,
                                              VecDbl& a_tVals)
{
  // current segment length
  double length(a_segLengths[a_idx]);
  // tvalues at the begin and end of current segment
  double t1(a_segTvalues[a_idx]), t2(a_segTvalues[a_nextIdx]);
  if (0 == a_nextIdx)
    t2 = 1;
  // target segment length at begin and end of segment
  double w1(a_interpLengths[a_idx]), w2(a_interpLengths[a_nextIdx]);

  bool done(false);
  while (!done)
  {
    double pcntleft = a_pcntin + (2 * length) / (w1 + w2);
    if (pcntleft >= 1.0)
    {
      double scale = 1.0 - a_pcntin;
      double ptlength = 2 * scale * w1 * length / (2 * length + scale * (w1 - w2));
      double pcnt = ptlength / length;
      double t = (pcnt * (t2 - t1)) + t1; // convert to polyline tval
      a_tVals.push_back(t);

      w1 = w1 * (1 - pcnt) + w2 * pcnt;
      t1 = t;
      length -= ptlength;
      a_pcntin = 0;
    }
    else
    {
      a_pcntin = pcntleft;
      done = true;
    }
  }
} // MePolyRedistributePtsImpl::TvaluesForSeg
//------------------------------------------------------------------------------
/// \brief Calculates average edge lengths at each point in a loop defining
/// a polygon.
/// \param a_outPoly Outside polygon
/// \param a_inPolys Inside polygons that are in a_outPoly
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::SizeFromPolyCalcAveEdgeLengthAtPts(const VecPt3d& a_outPoly,
                                                                   const VecPt3d2d& a_inPolys)
{
  m_minLength = XM_DBL_HIGHEST;
  m_maxLength = XM_DBL_LOWEST;
  SizeFromPolyAddEdgeLengths(a_outPoly);
  for (size_t i = 0; i < a_inPolys.size(); ++i)
    SizeFromPolyAddEdgeLengths(a_inPolys[i]);
} // MePolyRedistributePtsImpl::SizeFromPolyCalcAveEdgeLengthAtPts
//------------------------------------------------------------------------------
/// \brief Calculates average edge lengths at each point in a loop defining
/// a polygon.
/// \param a_pts Locations that define a polygon
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::SizeFromPolyAddEdgeLengths(const VecPt3d& a_pts)
{
  double l(0), dx, dy;
  size_t start = m_polyEdgeLengths.size();
  m_polyEdgeLengths.insert(m_polyEdgeLengths.end(), a_pts.size(), 0);
  for (size_t i = 0; i < a_pts.size(); ++i)
  {
    size_t nextIdx(0);
    Pt3d nextPt(a_pts[0]);
    if (i + 1 < a_pts.size())
    {
      nextIdx = i + 1;
      nextPt = a_pts[i + 1];
    }
    m_polyPts->push_back(a_pts[i]);

    dx = a_pts[i].x - nextPt.x;
    dy = a_pts[i].y - nextPt.y;
    l = sqrt(dx * dx + dy * dy);
    if (l < m_minLength)
      m_minLength = l;
    if (l > m_maxLength)
      m_maxLength = l;
    l = l / 2;
    m_polyEdgeLengths[i + start] += l;
    m_polyEdgeLengths[nextIdx + start] += l;
  }
} // MePolyRedistributePtsImpl::SizeFromPolyAddEdgeLengths
//------------------------------------------------------------------------------
/// \brief Calculates the lengths of segments and parametric values of segment
/// endpoints.
/// \param a_pts Vector of locations.
/// \param a_segLength Vector of segment lengths. Filled by this method.
/// \param a_segTvalues Vector of parametric values of segment endpoints
/// relative to the perimeter of the closed loop. Filled by this method.
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::CalcSegLengths(const VecPt3d& a_pts,
                                               VecDbl& a_segLength,
                                               VecDbl& a_segTvalues)
{
  a_segLength.assign(a_pts.size() - 1, 0);
  a_segTvalues.assign(a_pts.size() - 1, 0);
  double totalLength(0);
  for (size_t i = 1; i < a_pts.size(); ++i)
  {
    const Pt3d &p0(a_pts[i - 1]), &p1(a_pts[i]);
    a_segLength[i - 1] = Mdist(p0.x, p0.y, p1.x, p1.y);
    totalLength += a_segLength[i - 1];
  }
  double sumLen(0);
  for (size_t i = 1; i < a_pts.size() - 1; ++i)
  {
    sumLen += a_segLength[i - 1];
    a_segTvalues[i] = sumLen / totalLength;
  }
} // MePolyRedistributePtsImpl::CalcSegLengths
//------------------------------------------------------------------------------
/// \brief Finds the segment that contains a_tVal
/// \param a_segTvalues Vector of parametric values of segment endpoints
/// relative to the perimeter of the closed loop.
/// \param a_tVal Parametric value along closed loop.
/// \param a_segIdx Index for starting the search of a_segTvalues and can be
/// modified by this method when the segment is found.
/// \param a_segT0 The parametric t value for the start of the found segment
/// \param a_segT1 The parametric t value for the end of the found segment
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::GetSegmentFromTval(const VecDbl& a_segTvalues,
                                                   double a_tVal,
                                                   size_t& a_segIdx,
                                                   double& a_segT0,
                                                   double& a_segT1)
{
  if (a_tVal > 1)
  {
    XM_ASSERT(0);
    a_tVal = 1;
  }
  if (a_segIdx >= a_segTvalues.size())
    a_segIdx = 0;

  a_segT0 = a_segTvalues[a_segIdx];
  a_segT1 = 1;
  if (a_segIdx + 1 < a_segTvalues.size())
    a_segT1 = a_segTvalues[a_segIdx + 1];

  bool done = false;
  while (!done)
  {
    if (a_tVal >= a_segT0 && a_tVal <= a_segT1)
      done = true;
    else
    {
      a_segIdx++;
      a_segT0 = a_segTvalues[a_segIdx];
      a_segT1 = 1;
      if (a_segIdx + 1 < a_segTvalues.size())
        a_segT1 = a_segTvalues[a_segIdx + 1];
    }
  }
} // MePolyRedistributePtsImpl::GetSegmentFromTval
//------------------------------------------------------------------------------
/// \brief Creates the poly intersector class member variable
//------------------------------------------------------------------------------
void MePolyRedistributePtsImpl::CreatePolyIntersector()
{
  m_polys.assign(m_sizeTris->size() / 3, VecInt(3, 0));
  int* iPtr(&(*m_sizeTris)[0]);
  size_t idx;
  for (size_t i = 0; i < m_polys.size(); ++i)
  {
    idx = i * 3;
    m_polys[i][0] = iPtr[idx + 0];
    m_polys[i][1] = iPtr[idx + 1];
    m_polys[i][2] = iPtr[idx + 2];
  }
  BSHP<GmMultiPolyIntersectionSorterTerse> sorterTerse =
    boost::make_shared<GmMultiPolyIntersectionSorterTerse>();
  BSHP<GmMultiPolyIntersectionSorter> sorter = BDPC<GmMultiPolyIntersectionSorter>(sorterTerse);
  m_polyIntersector = GmMultiPolyIntersector::New(*m_sizePts, m_polys, sorter);

} // MePolyRedistributePtsImpl::CreatePolyIntersector
//------------------------------------------------------------------------------
/// \brief Free function access an implement method that needs to be hidden
/// from the public interface
/// \param[in] a_redist: a MePolyRedistributePts class
/// \param a_input Input closed loop polylines
/// \param a_out Redistributed closed loop polylines
/// \param a_polyOffsetIter Number of iterations from the polygon boundary
//------------------------------------------------------------------------------
void mePolyPaverRedistribute(BSHP<MePolyRedistributePts> a_redist,
                             const MePolyOffsetterOutput& a_input,
                             MePolyOffsetterOutput& a_out,
                             int a_polyOffsetIter)
{
  BSHP<MePolyRedistributePtsImpl> r = BDPC<MePolyRedistributePtsImpl>(a_redist);
  XM_ENSURE_TRUE(r);
  r->Redistribute(a_input, a_out, a_polyOffsetIter);
} // mePolyPaverRedistribute

} // namespace xms

#ifdef CXX_TEST
////////////////////////////////////////////////////////////////////////////////

#include <xmsmesh/meshing/MePolyRedistributePts.t.h>

#include <xmscore/testing/TestTools.h>
#include <xmsinterp/geometry/geoms.h>

// namespace xms {
using namespace xms;

////////////////////////////////////////////////////////////////////////////////
/// \class MePolyRedistributePtsUnitTests
/// \brief tester for the MePolyRedistributePts class
////////////////////////////////////////////////////////////////////////////////
//------------------------------------------------------------------------------
/// \brief tests creating the class
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testCreateClass()
{
  BSHP<MePolyRedistributePts> b = MePolyRedistributePts::New();
  TS_ASSERT(b);
} // MePolyRedistributePtsUnitTests::testCreateClass
//------------------------------------------------------------------------------
/// \brief tests interpolating the edge length
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testInterpEdgeLengths()
{
  MePolyRedistributePtsImpl r;
  // input polygon
  VecPt3d outPoly = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  VecPt3d2d inPolys;
  r.SetSizeFuncFromPoly(outPoly, inPolys, 1);
  VecPt3d pts = {{1, 1, 0}, {1, 9, 0}, {9, 9, 0}, {9, 1, 0}};
  VecDbl lengths;
  r.InterpEdgeLengths(pts, lengths);
  VecDbl baseLengths = {10.0, 10.0, 10.0, 10.0};
  TS_ASSERT_EQUALS_VEC(baseLengths, lengths);
} // MePolyRedistributePtsUnitTests::testInterpEdgeLengths
//------------------------------------------------------------------------------
/// \brief tests interpolating the edge length
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testInterpEdgeLengths1()
{
  MePolyRedistributePtsImpl r;
  // input polygon
  VecPt3d outPoly = {{0, 0, 0}, {0, 2, 0},  {0, 4, 0},   {0, 6, 0},
                     {0, 8, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  VecPt3d2d inPolys;
  r.SetSizeFuncFromPoly(outPoly, inPolys, 1);
  VecPt3d pts = {{1, 1, 0}, {1, 9, 0}, {9, 9, 0}, {9, 1, 0}};
  VecDbl lengths;
  r.InterpEdgeLengths(pts, lengths);
  VecDbl baseLengths = {5.00892, 5.00892, 9.79527, 9.79527};
  TS_ASSERT_DELTA_VEC(baseLengths, lengths, 1e-5);
} // MePolyRedistributePtsUnitTests::testInterpEdgeLengths1
//------------------------------------------------------------------------------
/// \brief tests interpolating the edge length
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testInterpEdgeLengths2()
{
  MePolyRedistributePtsImpl r;
  // input polygon
  VecPt3d outPoly = {{0, 0, 0}, {0, 2, 0},  {0, 4, 0},   {0, 6, 0},
                     {0, 8, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  VecPt3d2d inPolys;
  VecPt3d in = {{4.9, 5, 0}, {4.95, 4.9, 0}, {5.05, 4.9, 0},
                {5.1, 5, 0}, {5.50, 5.1, 0}, {4.95, 5.1, 0}};
  inPolys.push_back(in);

  r.SetSizeFuncFromPoly(outPoly, inPolys, 1);
  VecPt3d pts = {{1, 1, 0}, {1, 9, 0}, {9, 9, 0}, {9, 1, 0}};
  VecDbl lengths;
  r.InterpEdgeLengths(pts, lengths);
  VecDbl baseLengths = {4.96661, 4.96586, 9.71356, 9.71552};
  TS_ASSERT_DELTA_VEC(baseLengths, lengths, 1e-5);
  r.m_sizeBias = 0;
  r.InterpEdgeLengths(pts, lengths);
  baseLengths = {3.36034, 3.36167, 7.02831, 7.03054};
  TS_ASSERT_DELTA_VEC(baseLengths, lengths, 1e-5);
} // MePolyRedistributePtsUnitTests::testInterpEdgeLengths2
//------------------------------------------------------------------------------
/// \brief tests interpolating the edge length
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testInterpEdgeLengths3()
{
  MePolyRedistributePtsImpl r;
  r.SetConstantSizeFunc(5.0);
  VecPt3d pts = {{0, 0, 0}, {0, 100, 0}, {100, 100, 0}, {100, 0, 0}};
  VecDbl lengths;
  r.InterpEdgeLengths(pts, lengths);
  VecDbl baseLengths(4, 5.0);
  TS_ASSERT_EQUALS_VEC(baseLengths, lengths);
} // MePolyRedistributePtsUnitTests::testInterpEdgeLengths3
//------------------------------------------------------------------------------
/// \brief tests interpolating the edge length
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testInterpEdgeLengths4()
{
  MePolyRedistributePtsImpl r;
  // r.SetConstantSizeFunc(5.0);
  VecPt3d pts = {{0, 0, 0}, {0, 100, 0}, {100, 100, 0}, {100, 0, 0}};
  VecDbl lengths;
  r.InterpEdgeLengths(pts, lengths);
  VecDbl baseLengths(4, 100.0);
  TS_ASSERT_EQUALS_VEC(baseLengths, lengths);
} // MePolyRedistributePtsUnitTests::testInterpEdgeLengths4
//------------------------------------------------------------------------------
/// \brief test redistributing the points on the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPts()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}, {0, 0, 0}};
  VecDbl lengths = {5, 5, 5, 5, 5};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0},  {0, 5, 0},  {0, 10, 0}, {5, 10, 0}, {10, 10, 0},
                     {10, 5, 0}, {10, 0, 0}, {5, 0, 0},  {0, 0, 0}};
  TS_ASSERT_EQUALS_VEC(basePts, outPts);
} // MePolyRedistributePtsUnitTests::testRedistPts
//------------------------------------------------------------------------------
/// \brief test redistributing the points on the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPts1()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}, {0, 0, 0}};
  VecDbl lengths = {4, 4, 4, 4, 4};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0},  {0, 4, 0},  {0, 8, 0}, {2, 10, 0}, {6, 10, 0}, {10, 10, 0},
                     {10, 6, 0}, {10, 2, 0}, {8, 0, 0}, {4, 0, 0},  {0, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 1e-9);
} // MePolyRedistributePtsUnitTests::testRedistPts1
//------------------------------------------------------------------------------
/// \brief test redistributing the points on the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPts2()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}, {0, 0, 0}};
  VecDbl lengths = {3, 3, 3, 3, 3};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0},     {0, 3.08, 0},  {0, 6.15, 0},  {0, 9.23, 0},  {2.31, 10, 0},
                     {5.38, 10, 0}, {8.46, 10, 0}, {10, 8.46, 0}, {10, 5.38, 0}, {10, 2.31, 0},
                     {9.23, 0, 0},  {6.15, 0, 0},  {3.08, 0, 0},  {0, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPts2
//------------------------------------------------------------------------------
/// \brief test redistributing the points on the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPts3()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}, {0, 0, 0}};
  VecDbl lengths = {3, 5, 3, 5, 3};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0},     {0, 3.34, 0},  {0, 7.43, 0},  {2.2, 10, 0},
                     {6.36, 10, 0}, {9.76, 10, 0}, {10, 6.95, 0}, {10, 2.95, 0},
                     {8.19, 0, 0},  {3.95, 0, 0},  {0, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPts3
//------------------------------------------------------------------------------
/// \brief test redistributing the points on the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPts4()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}, {0, 0, 0}};
  VecDbl lengths = {15, 15, 15, 15, 15};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0}, {3.33, 10, 0}, {10, 3.33, 0}, {0, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPts4
//------------------------------------------------------------------------------
/// \brief test redistributing the points on the polygon boundary
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPts5()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}, {0, 0, 0}};
  VecDbl lengths = {20, 20, 20, 20, 20};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0}, {3.33, 10, 0}, {10, 3.33, 0}, {0, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPts4
//------------------------------------------------------------------------------
/// \brief tests intersecting the polygon with the triangles from the interp
/// size function
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testIntersectWithTris()
{
  VecPt3d loop = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  BSHP<VecPt3d> triPts(new VecPt3d());
  *triPts = {{5, 5, 0},   {-5, -5, 0}, {5, -5, 0},  {15, -5, 0}, {15, 5, 0},
             {15, 15, 0}, {5, 15, 0},  {-5, 15, 0}, {-5, 5, 0}};
  BSHP<VecInt> triTris(new VecInt());
  *triTris = {0, 1, 2, 0, 2, 3, 0, 3, 4, 0, 4, 5, 0, 5, 6, 0, 6, 7, 0, 7, 8, 0, 8, 1};

  MePolyRedistributePtsImpl r;
  r.m_sizePts = triPts;
  r.m_sizeTris = triTris;
  r.CreatePolyIntersector();
  r.IntersectWithTris(loop);
  VecPt3d baseLoop = {{0, 0, 0},   {0, 5, 0},  {0, 10, 0}, {5, 10, 0},
                      {10, 10, 0}, {10, 5, 0}, {10, 0, 0}, {5, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(baseLoop, loop, FLT_EPSILON);
} // MePolyRedistributePtsUnitTests::testIntersectWithTris
//------------------------------------------------------------------------------
/// \brief test redistributing the points on a polyline
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPolyLine()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  VecDbl lengths = {5, 5, 5, 5};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0},   {0, 5, 0},  {0, 10, 0}, {5, 10, 0},
                     {10, 10, 0}, {10, 5, 0}, {10, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPolyLine
//------------------------------------------------------------------------------
/// \brief test redistributing the points on a polyline
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPolyLine1()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  VecDbl lengths = {6, 6, 6, 6};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0}, {0, 6, 0}, {2, 10, 0}, {8, 10, 0}, {10, 6, 0}, {10, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPolyLine1
//------------------------------------------------------------------------------
/// \brief test redistributing the points on a polyline
//------------------------------------------------------------------------------
void MePolyRedistributePtsUnitTests::testRedistPolyLine2()
{
  VecPt3d pts = {{0, 0, 0}, {0, 10, 0}, {10, 10, 0}, {10, 0, 0}};
  VecDbl lengths = {3, 5, 3, 5};

  MePolyRedistributePtsImpl r;
  VecPt3d outPts = r.RedistPts(pts, lengths);
  VecPt3d basePts = {{0, 0, 0},     {0, 3.42, 0},  {0, 7.58, 0},  {2.43, 10, 0}, {6.66, 10, 0},
                     {10, 9.85, 0}, {10, 6.76, 0}, {10, 3.68, 0}, {10, 0, 0}};
  TS_ASSERT_DELTA_VECPT3D(basePts, outPts, 0.01);
} // MePolyRedistributePtsUnitTests::testRedistPolyLine2

//} // namespace xms
#endif


#include <scitbx/array_family/boost_python/flex_fwd.h>
#include <boost/python.hpp>
#include <spotfinder/core_toolbox/hough.h>

using namespace boost::python;

BOOST_PYTHON_MODULE(spotfinder_hough_ext)
{

    typedef return_value_policy<return_by_value> rbv;

    class_<spotfinder::hough>("hough",init<>())
      .def("importData",&spotfinder::hough::importData)
      .def("exportData",&spotfinder::hough::exportData)
      .def("setGeometry",&spotfinder::hough::setGeometry)
      .def("getGeometry",&spotfinder::hough::getGeometry)
      .def("gaussianBlur",&spotfinder::hough::gaussianBlur)
      .def("sobelEdge",&spotfinder::hough::sobelEdge)
      .def("cannyEdge",&spotfinder::hough::cannyEdge)
      .def("findEllipse",&spotfinder::hough::findEllipse)
      .def("getDistance",&spotfinder::hough::getDistance)
      .def("getRings",&spotfinder::hough::getRings)
      ;
}


// Copyright (c) 2016 Baidu.com, Inc. All Rights Reserved
// @author erlangz(zhengwenchao@baidu.com)
// @date 2016/12/22 16:13:51
// @file types.cpp
// @brief

#include "pcd/types.h"

#include <boost/typeof/typeof.hpp>
#include <boost/foreach.hpp>
#include <pcl/filters/extract_indices.h>

namespace adu {
namespace perception {

Box::Box(const std::string& file_, int id_, const pt::ptree& root) {
    file = file_;
    id = id_;
    //Rotation
    BOOST_AUTO(r, root.get_child("rotation"));
    //phi - Z axis; theta - X axis; psi - Y axis
    rotation_z = Eigen::AngleAxisd(r.get<double>("phi"), Eigen::Vector3d::UnitZ());
    rotation_x = Eigen::AngleAxisd(r.get<double>("theta"), Eigen::Vector3d::UnitX());
    rotation_y = Eigen::AngleAxisd(r.get<double>("psi"), Eigen::Vector3d::UnitY());

    std::vector<double> size;
    BOOST_FOREACH(const pt::ptree::value_type& item, root.get_child("size")) {
        size.push_back(item.second.get_value<double>());
    }
    //AlignedBox
    BOOST_AUTO(center, root.get_child("position"));
    bounding_box = Eigen::AlignedBox3d(Eigen::Vector3d(center.get<double>("x") - size[0]/2, center.get<double>("y") - size[1]/2, center.get<double>("z") - size[2]/2),
                                       Eigen::Vector3d(center.get<double>("x") + size[0]/2, center.get<double>("y") + size[1]/2, center.get<double>("z") + size[2]/2));

    //Type
    const std::string& type_str = root.get<std::string>("type");
    if (type_str == "smallMot") {
        type = smallMot;
    } else if (type_str == "pedestrian") {
        type = pedestrian;
    } else if (type_str == "bigMot") {
        type = bigMot;
    } else if (type_str == "midMot") {
        type = midMot;
    } else if (type_str == "nonMot") {
        type = nonMot;
    } else if (type_str == "unknow") {
        type = unknown;
    } else if (type_str == "cluster") {
        type = cluster;
    } else {
        std::cout << type_str << std::endl;
    }
}

void Box::show(pcl::visualization::PCLVisualizer& viewer) {
    Eigen::Vector3f color = get_color();
    viewer.addCube(bounding_box.min().x(), bounding_box.max().x(),
                   bounding_box.min().y(), bounding_box.max().y(),
                   bounding_box.min().z(), bounding_box.max().z(),
                   color(0), color(1), color(2),
                   id_str());
}

pcl::PointCloud<pcl::PointXYZ>::Ptr Box::get_cloud(pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud) {
    return BoxFilter::filter(point_cloud, BoxFilter::filter(point_cloud, *this));
}

std::string Box::type_str() const {
    switch (type) {
        case smallMot:
        case midMot:
        case bigMot: return "vehicle";
        case pedestrian: return "pedestrian";
        default: return "unknown";
    }
}
const std::string Box::id_str() const {
    std::stringstream ss;
    ss << file << "-" << type << "-" << id;
    return ss.str();
}

Eigen::Vector3f Box::get_color() const {
    switch (type) {
        case smallMot: return Eigen::Vector3f(0.0, 1.0, 0.0);
        case midMot: return Eigen::Vector3f(0.0, 0.8, 0.0);
        case bigMot: return Eigen::Vector3f(0.0, 0.9, 0.0);
        case nonMot: return Eigen::Vector3f(0.0, 0.7, 0.0);
        case pedestrian: return Eigen::Vector3f(1.0, 0.0, 0.0);
        case cluster: return Eigen::Vector3f(0.0, 0.5, 0.5);
        case unknown: return Eigen::Vector3f(0.0, 0.0, 1.0);
    }
    std::cerr << "PCD/Types.h Get Unknow Color:" << type << std::endl;
    return Eigen::Vector3f(0.0, 0.0, 0.0);
}

std::string Box::debug_string() const {
    std::stringstream ss;
    ss << "type:" << type
       << " box:(" << bounding_box.max().transpose() << ","<< bounding_box.min().transpose() << ")";
    return ss.str();
}

pcl::PointIndices::Ptr BoxFilter::filter(const pcl::PointCloud<pcl::PointXYZ>::Ptr& point_cloud, const Box& box) {
    pcl::PointIndices::Ptr indice(new pcl::PointIndices);
    for (size_t i = 0; i < point_cloud->size(); i++) {
        if (box.bounding_box.exteriorDistance(Eigen::Vector3d(point_cloud->at(i).x, point_cloud->at(i).y, point_cloud->at(i).z)) == 0) {
            indice->indices.push_back(i);
        }
    }
    return indice;
}

pcl::PointCloud<pcl::PointXYZ>::Ptr BoxFilter::filter(
        const pcl::PointCloud<pcl::PointXYZ>::Ptr& point_cloud,
        const pcl::PointIndices::Ptr& point_indice) {
    pcl::ExtractIndices<pcl::PointXYZ> eifilter(true);
    eifilter.setInputCloud(point_cloud);
    eifilter.setIndices(point_indice);

    pcl::PointCloud<pcl::PointXYZ>::Ptr output_point(new pcl::PointCloud<pcl::PointXYZ>);
    eifilter.filter(*output_point);
    return output_point;
}

Label::Label(const std::string& file, const pt::ptree& root) {
    file_name = file;
    int i = 0;
    BOOST_FOREACH(const pt::ptree::value_type& result, root.get_child("result")) {
        boxes.push_back(Box::Ptr(new Box(file_name, i++, result.second)));
    }
}

std::string Label::debug_string() const {
    std::stringstream ss;
    ss << "file_name:" << file_name
       << "Boxes:(" << boxes.size() << ")";
    for (size_t i = 0; i < boxes.size(); i++) {
        ss << "[" << boxes[i]->debug_string() << "]";
    }
    return ss.str();
}

} // namespace perception
} // namespace adu