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

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

#include "simd_pair_hmm.hpp"

#include <vector>
#include <algorithm>
#include <emmintrin.h>
#include <cassert>

#include <boost/container/small_vector.hpp>

//#include <iostream> // DEBUG
//#include <iterator> // DEBUG
//
//struct Seq { __m128i val; };
//struct Qual { __m128i val; };
//struct Mask { __m128i val; };
//
//std::ostream& operator<<(std::ostream& os, const Seq s)
//{
//    const auto val = (std::uint16_t*) &s.val;
//    std::copy(val, val + 8, std::ostreambuf_iterator<char>(os));
//    return os;
//}
//
//std::ostream& operator<<(std::ostream& os, const Qual q)
//{
//    const auto val = (std::uint16_t*) &q.val;
//    std::transform(val, val + 8, std::ostream_iterator<unsigned>(os, " "),
//                   [] (const auto x) { return x >> 2; });
//    return os;
//}
//
//std::ostream& operator<<(std::ostream& os, const Mask m)
//{
//    const auto val = (std::uint16_t*) &m.val;
//    std::copy(val, val + 8, std::ostream_iterator<bool>(os));
//    return os;
//}

namespace octopus { namespace hmm { namespace simd {

constexpr std::size_t staticBackpointerCapacity {10000};

template <typename T>
using SmallVector = boost::container::small_vector<T, staticBackpointerCapacity>;

constexpr short nScore {2 << 2};
constexpr int bandSize {8};
constexpr short inf {0x7800};
constexpr char gap {'-'};

auto extract_epi16(const __m128i a, const int imm) noexcept
{
    switch (imm) {
        case 0:  return _mm_extract_epi16(a, 0);
        case 1:  return _mm_extract_epi16(a, 1);
        case 2:  return _mm_extract_epi16(a, 2);
        case 3:  return _mm_extract_epi16(a, 3);
        case 4:  return _mm_extract_epi16(a, 4);
        case 5:  return _mm_extract_epi16(a, 5);
        case 6:  return _mm_extract_epi16(a, 6);
        default: return _mm_extract_epi16(a, 7);
    }
}

int align(const char* truth, const char* target, const std::int8_t* qualities,
          int truth_len, int target_len,
          short gap_open, short gap_extend, short nuc_prior) noexcept
{
    // target is the read; the shorter of the sequences
    // no checks for overflow are done
    //
    // the bottom-left and top-right corners of the DP table are just
    // included at the extreme ends of the diagonal, which measures
    // n=8 entries diagonally across.  This fixes the length of the
    // longer (horizontal) sequence to 15 (2*8-1) more than the shorter
    //
    // the << 2's are because the lower two bits are reserved for back tracing

    assert(truth_len > bandSize && (truth_len == target_len + 2 * bandSize - 1));

    gap_open <<= 2;
    gap_extend <<= 2;
    nuc_prior <<= 2;

    using SimdInt = __m128i;

    SimdInt _m1 {_mm_set1_epi16(inf)};
    auto _i1 = _m1;
    auto _d1 = _m1;
    auto _m2 = _m1;
    auto _i2 = _m1;
    auto _d2 = _m1;

    const SimdInt _gap_open {_mm_set1_epi16(gap_open)};
    const SimdInt _gap_extend {_mm_set1_epi16(gap_extend)};
    const SimdInt _nuc_prior  {_mm_set1_epi16(nuc_prior)};

    SimdInt _initmask   {_mm_set_epi16(0,0,0,0,0,0,0,-1)};
    SimdInt _initmask2  {_mm_set_epi16(0,0,0,0,0,0,0,-0x8000)};

    // truth is initialized with the n-long prefix, in forward direction
    // target is initialized as empty; reverse direction
    SimdInt _truthwin  {_mm_set_epi16(truth[7], truth[6], truth[5], truth[4],
                                      truth[3], truth[2], truth[1], truth[0])};
    SimdInt _targetwin  {_m1};
    SimdInt _qualitieswin {_mm_set1_epi16(64 << 2)};

    // if N, make nScore; if != N, make inf
    SimdInt _truthnqual {_mm_add_epi16(_mm_and_si128(_mm_cmpeq_epi16(_truthwin, _mm_set1_epi16('N')),
                                                     _mm_set1_epi16(nScore - inf)),
                                       _mm_set1_epi16(inf))};

    short minscore {inf};

    for (int s {0}; s <= 2 * (target_len + bandSize); s += 2) {
        // truth is current; target needs updating
        _targetwin    = _mm_slli_si128(_targetwin, 2);
        _qualitieswin = _mm_slli_si128(_qualitieswin, 2);

        if (s / 2 < target_len) {
            _targetwin    = _mm_insert_epi16(_targetwin, target[s / 2], 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, qualities[s / 2] << 2, 0);
        } else {
            _targetwin    = _mm_insert_epi16(_targetwin, '0', 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, 64 << 2, 0);
        }

        // S even

        _m1 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m1));
        _m2 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m2));
        _m1 = _mm_min_epi16(_m1, _mm_min_epi16(_i1, _d1));

        short score = extract_epi16(_m1, std::max(0, s / 2 - target_len));
        if (s / 2 >= target_len) {
            minscore = std::min(score, minscore);
        }

        _m1 = _mm_add_epi16(_m1, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _qualitieswin), _truthnqual));
        _d1 = _mm_min_epi16(_mm_add_epi16(_d2, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m2, _i2),
                                          _mm_srli_si128(_gap_open, 2))); // allow I->D
        _d1 = _mm_insert_epi16(_mm_slli_si128(_d1, 2), inf, 0);
        _i1 = _mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_i2, _gap_extend),
                                          _mm_add_epi16(_m2, _gap_open)),
                            _nuc_prior);

        // S odd
        // truth needs updating; target is current
        const auto pos = bandSize + s / 2;
        const char base {(pos < truth_len) ? truth[pos] : 'N'};

        _truthwin   = _mm_insert_epi16(_mm_srli_si128(_truthwin, 2), base,
                                       bandSize - 1);
        _truthnqual = _mm_insert_epi16(_mm_srli_si128(_truthnqual, 2), base == 'N' ? nScore : inf,
                                       bandSize - 1);

        _initmask  = _mm_slli_si128(_initmask, 2);
        _initmask2 = _mm_slli_si128(_initmask2, 2);
        _m2 = _mm_min_epi16(_m2, _mm_min_epi16(_i2, _d2));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m2, s / 2 - target_len)), minscore);
        }

        _m2 = _mm_add_epi16(_m2, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _qualitieswin), _truthnqual));
        _d2 = _mm_min_epi16(_mm_add_epi16(_d1, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m1, _i1), _gap_open)); // allow I->D
        _i2 = _mm_insert_epi16(_mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_mm_srli_si128(_i1, 2),
                                                                         _gap_extend),
                                                           _mm_add_epi16(_mm_srli_si128(_m1, 2),
                                                                         _gap_open)),
                                             _nuc_prior), inf, bandSize - 1);

    }

    return (minscore + 0x8000) >> 2;
}

int align(const char* truth, const char* target, const std::int8_t* qualities,
          const int truth_len, const int target_len,
          const std::int8_t* gap_open, short gap_extend, short nuc_prior) noexcept
{
    // target is the read; the shorter of the sequences
    // no checks for overflow are done
    //
    // the bottom-left and top-right corners of the DP table are just
    // included at the extreme ends of the diagonal, which measures
    // n=8 entries diagonally across.  This fixes the length of the
    // longer (horizontal) sequence to 15 (2*8-1) more than the shorter
    //
    // the << 2's are because the lower two bits are reserved for back tracing

    assert(truth_len > bandSize && (truth_len == target_len + 2 * bandSize - 1));

    gap_extend <<= 2;
    nuc_prior <<= 2;

    using SimdInt = __m128i;

    SimdInt _m1 {_mm_set1_epi16(inf)};
    auto _i1 = _m1;
    auto _d1 = _m1;
    auto _m2 = _m1;
    auto _i2 = _m1;
    auto _d2 = _m1;

    SimdInt _gap_extend {_mm_set1_epi16(gap_extend)};
    SimdInt _nuc_prior  {_mm_set1_epi16(nuc_prior)};

    SimdInt _initmask   {_mm_set_epi16(0,0,0,0,0,0,0,-1)};
    SimdInt _initmask2  {_mm_set_epi16(0,0,0,0,0,0,0,-0x8000)};

    // truth is initialized with the n-long prefix, in forward direction
    // target is initialized as empty; reverse direction
    SimdInt _truthwin  {_mm_set_epi16(truth[7], truth[6], truth[5], truth[4],
                                      truth[3], truth[2], truth[1], truth[0])};
    SimdInt _targetwin  {_m1};
    SimdInt _qualitieswin {_mm_set1_epi16(64 << 2)};

    // if N, make nScore; if != N, make inf
    SimdInt _truthnqual {_mm_add_epi16(_mm_and_si128(_mm_cmpeq_epi16(_truthwin, _mm_set1_epi16('N')),
                                                     _mm_set1_epi16(nScore - inf)),
                                       _mm_set1_epi16(inf))};

    SimdInt _gap_open {_mm_set_epi16(gap_open[7] << 2,gap_open[6] << 2,gap_open[5] << 2,gap_open[4] << 2,
                                     gap_open[3] << 2,gap_open[2] << 2,gap_open[1] << 2,gap_open[0] << 2)};

    short minscore {inf};

    for (int s {0}; s <= 2 * (target_len + bandSize); s += 2) {
        // truth is current; target needs updating
        _targetwin    = _mm_slli_si128(_targetwin, 2);
        _qualitieswin = _mm_slli_si128(_qualitieswin, 2);

        if (s / 2 < target_len) {
            _targetwin    = _mm_insert_epi16(_targetwin, target[s / 2], 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, qualities[s / 2] << 2, 0);
        } else {
            _targetwin    = _mm_insert_epi16(_targetwin, '0', 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, 64 << 2, 0);
        }

        // S even

        _m1 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m1));
        _m2 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m2));
        _m1 = _mm_min_epi16(_m1, _mm_min_epi16(_i1, _d1));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m1, s / 2 - target_len)), minscore);
        }

        _m1 = _mm_add_epi16(_m1, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _qualitieswin), _truthnqual));
        _d1 = _mm_min_epi16(_mm_add_epi16(_d2, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m2, _i2),
                                          _mm_srli_si128(_gap_open, 2))); // allow I->D
        _d1 = _mm_insert_epi16(_mm_slli_si128(_d1, 2), inf, 0);
        _i1 = _mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_i2, _gap_extend),
                                          _mm_add_epi16(_m2, _gap_open)),
                            _nuc_prior);

        // S odd
        // truth needs updating; target is current
        const auto pos = bandSize + s / 2;
        const char base {(pos < truth_len) ? truth[pos] : 'N'};

        _truthwin   = _mm_insert_epi16(_mm_srli_si128(_truthwin, 2), base,
                                       bandSize - 1);
        _truthnqual = _mm_insert_epi16(_mm_srli_si128(_truthnqual, 2), base == 'N' ? nScore : inf,
                                       bandSize - 1);
        _gap_open   = _mm_insert_epi16(_mm_srli_si128(_gap_open, 2),
                                       gap_open[pos < truth_len ? pos : truth_len - 1] << 2,
                                       bandSize - 1);

        _initmask  = _mm_slli_si128(_initmask, 2);
        _initmask2 = _mm_slli_si128(_initmask2, 2);

        _m2 = _mm_min_epi16(_m2, _mm_min_epi16(_i2, _d2));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m2, s / 2 - target_len)), minscore);
        }

        _m2 = _mm_add_epi16(_m2, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                 _qualitieswin), _truthnqual));
        _d2 = _mm_min_epi16(_mm_add_epi16(_d1, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m1, _i1), _gap_open)); // allow I->D
        _i2 = _mm_insert_epi16(_mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_mm_srli_si128(_i1, 2),
                                                                         _gap_extend),
                                                           _mm_add_epi16(_mm_srli_si128(_m1, 2),
                                                                         _gap_open)),
                                             _nuc_prior), inf, bandSize - 1);

    }

    return (minscore + 0x8000) >> 2;
}

int align(const char* truth, const char* target, const std::int8_t* qualities,
          const int truth_len, const int target_len,
          const std::int8_t* gap_open, const std::int8_t* gap_extend,
          short nuc_prior) noexcept
{
    assert(truth_len > bandSize && (truth_len == target_len + 2 * bandSize - 1));

    nuc_prior <<= 2;

    using SimdInt = __m128i;

    SimdInt _m1 {_mm_set1_epi16(inf)};
    auto _i1 = _m1;
    auto _d1 = _m1;
    auto _m2 = _m1;
    auto _i2 = _m1;
    auto _d2 = _m1;

    SimdInt _nuc_prior  {_mm_set1_epi16(nuc_prior)};

    SimdInt _initmask   {_mm_set_epi16(0,0,0,0,0,0,0,-1)};
    SimdInt _initmask2  {_mm_set_epi16(0,0,0,0,0,0,0,-0x8000)};

    // truth is initialized with the n-long prefix, in forward direction
    // target is initialized as empty; reverse direction
    SimdInt _truthwin  {_mm_set_epi16(truth[7], truth[6], truth[5], truth[4],
                                      truth[3], truth[2], truth[1], truth[0])};
    SimdInt _targetwin  {_m1};
    SimdInt _qualitieswin {_mm_set1_epi16(64 << 2)};

    // if N, make nScore; if != N, make inf
    SimdInt _truthnqual {_mm_add_epi16(_mm_and_si128(_mm_cmpeq_epi16(_truthwin, _mm_set1_epi16('N')),
                                                     _mm_set1_epi16(nScore - inf)),
                                       _mm_set1_epi16(inf))};

    SimdInt _gap_open {_mm_set_epi16(gap_open[7] << 2,gap_open[6] << 2,gap_open[5] << 2,gap_open[4] << 2,
                                     gap_open[3] << 2,gap_open[2] << 2,gap_open[1] << 2,gap_open[0] << 2)};
    SimdInt _gap_extend {_mm_set_epi16(gap_extend[7] << 2,gap_extend[6] << 2,gap_extend[5] << 2,gap_extend[4] << 2,
                                       gap_extend[3] << 2,gap_extend[2] << 2,gap_extend[1] << 2,gap_extend[0] << 2)};

    short minscore {inf};

    for (int s {0}; s <= 2 * (target_len + bandSize); s += 2) {
        // truth is current; target needs updating
        _targetwin    = _mm_slli_si128(_targetwin, 2);
        _qualitieswin = _mm_slli_si128(_qualitieswin, 2);

        if (s / 2 < target_len) {
            _targetwin    = _mm_insert_epi16(_targetwin, target[s / 2], 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, qualities[s / 2] << 2, 0);
        } else {
            _targetwin    = _mm_insert_epi16(_targetwin, '0', 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, 64 << 2, 0);
        }

        // S even
        _m1 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m1));
        _m2 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m2));
        _m1 = _mm_min_epi16(_m1, _mm_min_epi16(_i1, _d1));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m1, s / 2 - target_len)), minscore);
        }

        _m1 = _mm_add_epi16(_m1, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _qualitieswin), _truthnqual));
        _d1 = _mm_min_epi16(_mm_add_epi16(_d2, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m2, _i2),
                                          _mm_srli_si128(_gap_open, 2))); // allow I->D
        _d1 = _mm_insert_epi16(_mm_slli_si128(_d1, 2), inf, 0);
        _i1 = _mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_i2, _gap_extend),
                                          _mm_add_epi16(_m2, _gap_open)),
                            _nuc_prior);

        // S odd; truth needs updating; target is current
        const auto pos = bandSize + s / 2;
        const char base {(pos < truth_len) ? truth[pos] : 'N'};
        _truthwin   = _mm_insert_epi16(_mm_srli_si128(_truthwin, 2), base, bandSize - 1);
        _truthnqual = _mm_insert_epi16(_mm_srli_si128(_truthnqual, 2), base == 'N' ? nScore : inf, bandSize - 1);
        const auto gap_idx = pos < truth_len ? pos : truth_len - 1;
        _gap_open   = _mm_insert_epi16(_mm_srli_si128(_gap_open, 2), gap_open[gap_idx] << 2, bandSize - 1);
        _gap_extend = _mm_insert_epi16(_mm_srli_si128(_gap_extend, 2), gap_extend[gap_idx] << 2, bandSize - 1);

        _initmask  = _mm_slli_si128(_initmask, 2);
        _initmask2 = _mm_slli_si128(_initmask2, 2);

        _m2 = _mm_min_epi16(_m2, _mm_min_epi16(_i2, _d2));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m2, s / 2 - target_len)), minscore);
        }

        _m2 = _mm_add_epi16(_m2, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _qualitieswin), _truthnqual));
        _d2 = _mm_min_epi16(_mm_add_epi16(_d1, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m1, _i1), _gap_open)); // allow I->D
        _i2 = _mm_insert_epi16(_mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_mm_srli_si128(_i1, 2),
                                                                         _gap_extend),
                                                           _mm_add_epi16(_mm_srli_si128(_m1, 2),
                                                                         _gap_open)),
                                             _nuc_prior), inf, bandSize - 1);

    }

    return (minscore + 0x8000) >> 2;
}

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 std::int8_t* gap_open, short gap_extend, short nuc_prior) noexcept
{
    assert(truth_len > bandSize && (truth_len == target_len + 2 * bandSize - 1));

    gap_extend <<= 2;
    nuc_prior <<= 2;

    using SimdInt = __m128i;

    SimdInt _m1 {_mm_set1_epi16(inf)};
    auto _i1 = _m1;
    auto _d1 = _m1;
    auto _m2 = _m1;
    auto _i2 = _m1;
    auto _d2 = _m1;

    SimdInt _gap_extend {_mm_set1_epi16(gap_extend)};
    SimdInt _nuc_prior  {_mm_set1_epi16(nuc_prior)};
    SimdInt _initmask   {_mm_set_epi16(0,0,0,0,0,0,0,-1)};
    SimdInt _initmask2  {_mm_set_epi16(0,0,0,0,0,0,0,-0x8000)};

    SimdInt _truthwin  {_mm_set_epi16(truth[7], truth[6], truth[5], truth[4],
                                      truth[3], truth[2], truth[1], truth[0])};
    SimdInt _targetwin  {_m1};
    SimdInt _qualitieswin {_mm_set1_epi16(64 << 2)};

    SimdInt _snvmaskwin  {_mm_set_epi16(snv_mask[7], snv_mask[6], snv_mask[5], snv_mask[4],
                                        snv_mask[3], snv_mask[2], snv_mask[1], snv_mask[0])};
    SimdInt _snv_priorwin {_mm_set_epi16(snv_prior[7] << 2, snv_prior[6] << 2, snv_prior[5] << 2, snv_prior[4] << 2,
                                         snv_prior[3] << 2, snv_prior[2] << 2, snv_prior[1] << 2, snv_prior[0] << 2)};

    SimdInt _snvmask;

    // if N, make nScore; if != N, make inf
    SimdInt _truthnqual {_mm_add_epi16(_mm_and_si128(_mm_cmpeq_epi16(_truthwin, _mm_set1_epi16('N')),
                                                     _mm_set1_epi16(nScore - inf)),
                                       _mm_set1_epi16(inf))};

    SimdInt _gap_open {_mm_set_epi16(gap_open[7] << 2,gap_open[6] << 2,gap_open[5] << 2,gap_open[4] << 2,
                                     gap_open[3] << 2,gap_open[2] << 2,gap_open[1] << 2,gap_open[0] << 2)};

    short minscore {inf};

    for (int s {0}; s <= 2 * (target_len + bandSize); s += 2) {
        // truth is current; target needs updating
        _targetwin    = _mm_slli_si128(_targetwin, 2);
        _qualitieswin = _mm_slli_si128(_qualitieswin, 2);

        if (s / 2 < target_len) {
            _targetwin    = _mm_insert_epi16(_targetwin, target[s / 2], 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, qualities[s / 2] << 2, 0);
        } else {
            _targetwin    = _mm_insert_epi16(_targetwin, '0', 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, 64 << 2, 0);
        }

        // S even

        _m1 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m1));
        _m2 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m2));
        _m1 = _mm_min_epi16(_m1, _mm_min_epi16(_i1, _d1));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m1, s / 2 - target_len)), minscore);
        }

        _snvmask = _mm_cmpeq_epi16(_targetwin, _snvmaskwin);

        _m1 = _mm_add_epi16(_m1, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                 _mm_min_epi16(_qualitieswin,
                                                                               _mm_or_si128(_mm_and_si128(_snvmask, _snv_priorwin),
                                                                                _mm_andnot_si128(_snvmask, _qualitieswin)))),
                                               _truthnqual));
        _d1 = _mm_min_epi16(_mm_add_epi16(_d2, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m2, _i2),
                                          _mm_srli_si128(_gap_open, 2))); // allow I->D
        _d1 = _mm_insert_epi16(_mm_slli_si128(_d1, 2), inf, 0);
        _i1 = _mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_i2, _gap_extend),
                                          _mm_add_epi16(_m2, _gap_open)),
                            _nuc_prior);

        // S odd
        // truth needs updating; target is current
        const auto pos = bandSize + s / 2;
        const char base {pos < truth_len ? truth[pos] : 'N'};

        _truthwin     = _mm_insert_epi16(_mm_srli_si128(_truthwin, 2),
                                         base,
                                         bandSize - 1);
        _truthnqual   = _mm_insert_epi16(_mm_srli_si128(_truthnqual, 2),
                                         base == 'N' ? nScore : inf,
                                         bandSize - 1);
        _snvmaskwin   = _mm_insert_epi16(_mm_srli_si128(_snvmaskwin, 2),
                                         pos < truth_len ? snv_mask[pos] : 'N',
                                         bandSize - 1);
        _snv_priorwin = _mm_insert_epi16(_mm_srli_si128(_snv_priorwin, 2),
                                         (pos < truth_len ? snv_prior[pos] : inf) << 2,
                                         bandSize - 1);
        _gap_open     = _mm_insert_epi16(_mm_srli_si128(_gap_open, 2),
                                         gap_open[pos < truth_len ? pos : truth_len - 1] << 2,
                                         bandSize - 1);

        _initmask  = _mm_slli_si128(_initmask, 2);
        _initmask2 = _mm_slli_si128(_initmask2, 2);

        _m2 = _mm_min_epi16(_m2, _mm_min_epi16(_i2, _d2));

        if (s / 2 >= target_len) {
            minscore = std::min(static_cast<short>(extract_epi16(_m2, s / 2 - target_len)), minscore);
        }

        _snvmask = _mm_cmpeq_epi16(_targetwin, _snvmaskwin);

        _m2 = _mm_add_epi16(_m2, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _mm_min_epi16(_qualitieswin,
                                                                              _mm_or_si128(_mm_and_si128(_snvmask, _snv_priorwin),
                                                                                _mm_andnot_si128(_snvmask, _qualitieswin)))),
                                               _truthnqual));
        _d2 = _mm_min_epi16(_mm_add_epi16(_d1, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m1, _i1), _gap_open)); // allow I->D
        _i2 = _mm_insert_epi16(_mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_mm_srli_si128(_i1, 2),
                                                                         _gap_extend),
                                                           _mm_add_epi16(_mm_srli_si128(_m1, 2),
                                                                         _gap_open)),
                                             _nuc_prior), inf, bandSize - 1);
    }

    return (minscore + 0x8000) >> 2;
}

int align(const char* truth, const char* target, const std::int8_t* qualities,
          const int truth_len, const int target_len,
          const std::int8_t* gap_open, short gap_extend, short nuc_prior,
          int& first_pos, char* aln1, char* aln2) noexcept
{
    // target is the read; the shorter of the sequences
    // no checks for overflow are done

    // the bottom-left and top-right corners of the DP table are just
    // included at the extreme ends of the diagonal, which measures
    // n=8 entries diagonally across.  This fixes the length of the
    // longer (horizontal) sequence to 15 (2*8-1) more than the shorter

    assert(truth_len > bandSize && (truth_len == target_len + 2 * bandSize - 1));
    assert(aln1 != nullptr && aln2 != nullptr);

    gap_extend <<= 2;
    nuc_prior <<= 2;

    constexpr int matchLabel  {0};
    constexpr int insertLabel {1};
    constexpr int deleteLabel {3};

    using SimdInt = __m128i;

    SimdInt _m1 {_mm_set1_epi16(inf)};
    auto _i1 = _m1;
    auto _d1 = _m1;
    auto _m2 = _m1;
    auto _i2 = _m1;
    auto _d2 = _m1;

    SimdInt _gap_extend {_mm_set1_epi16(gap_extend)};
    SimdInt _nuc_prior  {_mm_set1_epi16(nuc_prior)};
    SimdInt _initmask   {_mm_set_epi16(0,0,0,0,0,0,0,-1)};
    SimdInt _initmask2  {_mm_set_epi16(0,0,0,0,0,0,0,-0x8000)};

    static const SimdInt _three {_mm_set1_epi16(3)};
    SmallVector<SimdInt> _backpointers(2 * (truth_len + bandSize));

    // sequence 1 is initialized with the n-long prefix, in forward direction
    // sequence 2 is initialized as empty; reverse direction
    SimdInt _truthwin  {_mm_set_epi16(truth[7], truth[6], truth[5], truth[4],
                                      truth[3], truth[2], truth[1], truth[0])};
    SimdInt _targetwin  {_m1};
    SimdInt _qualitieswin {_mm_set1_epi16(64 << 2)};

    // if N, make nScore; if != N, make inf
    SimdInt _truthnqual {_mm_add_epi16(_mm_and_si128(_mm_cmpeq_epi16(_truthwin, _mm_set1_epi16('N')),
                                                     _mm_set1_epi16(nScore - inf)),
                                       _mm_set1_epi16(inf))};

    SimdInt _gap_open {_mm_set_epi16(gap_open[7] << 2,gap_open[6] << 2,gap_open[5] << 2,gap_open[4] << 2,
                                     gap_open[3] << 2,gap_open[2] << 2,gap_open[1] << 2,gap_open[0] << 2)};

    short cur_score {0}, minscore {inf}, minscoreidx {-1};

    // main loop.  Do one extra iteration, with nucs from sequence 2 just moved out
    // of the targetwin/qual arrays, to simplify getting back pointers
    int s;
    for (s = 0; s <= 2 * (target_len + bandSize); s += 2) {
        // truth is current; target needs updating
        _targetwin    = _mm_slli_si128(_targetwin, 2);
        _qualitieswin = _mm_slli_si128(_qualitieswin, 2);

        if (s / 2 < target_len) {
            _targetwin    = _mm_insert_epi16(_targetwin, target[s / 2], 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, qualities[s / 2] << 2, 0);
        } else {
            _targetwin    = _mm_insert_epi16(_targetwin, '0', 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, 64 << 2, 0);
        }

        // S even
        _m1 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m1));
        _m2 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m2));
        _m1 = _mm_min_epi16(_m1, _mm_min_epi16(_i1, _d1));

        if (s / 2 >= target_len) {
            cur_score = extract_epi16(_m1, s / 2 - target_len);
            if (cur_score < minscore) {
                minscore = cur_score;
                minscoreidx = s;     // point back to the match state at this entry, so as not to
            }                        // have to store the state at s-2
        }

        _m1 = _mm_add_epi16(_m1, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _qualitieswin), _truthnqual));
        _d1 = _mm_min_epi16(_mm_add_epi16(_d2, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m2, _i2),
                                          _mm_srli_si128(_gap_open, 2))); // allow I->D
        _d1 = _mm_insert_epi16(_mm_slli_si128(_d1, 2), inf, 0);
        _i1 = _mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_i2, _gap_extend),
                                          _mm_add_epi16(_m2, _gap_open)), _nuc_prior);

        _backpointers[s] = _mm_or_si128(_mm_or_si128(_mm_and_si128(_three, _m1),
                                                     _mm_slli_epi16(_mm_and_si128(_three, _i1), 2 * insertLabel)),
                                        _mm_slli_epi16(_mm_and_si128(_three, _d1), 2 * deleteLabel));

        // set state labels
        _m1 = _mm_andnot_si128(_three, _m1);
        _i1 = _mm_or_si128(_mm_andnot_si128(_three, _i1), _mm_srli_epi16(_three, 1));
        _d1 = _mm_or_si128(_mm_andnot_si128(_three, _d1), _three);

        // S odd

        // truth needs updating; target is current
        const char c {(bandSize + s / 2 < truth_len) ? truth[bandSize + (s / 2)] : 'N'};

        _truthwin   = _mm_insert_epi16(_mm_srli_si128(_truthwin,   2), c, bandSize - 1);
        _truthnqual = _mm_insert_epi16(_mm_srli_si128(_truthnqual, 2), (c == 'N') ? nScore : inf, bandSize - 1);
        _gap_open   = _mm_insert_epi16(_mm_srli_si128(_gap_open,  2),
                                       gap_open[bandSize + s / 2 < truth_len ? bandSize + s / 2 : truth_len - 1] << 2, bandSize - 1);
        _initmask  = _mm_slli_si128(_initmask, 2);
        _initmask2 = _mm_slli_si128(_initmask2, 2);
        _m2 = _mm_min_epi16(_m2, _mm_min_epi16(_i2, _d2));

        // at this point, extract minimum score.  Referred-to position must
        // be y==target_len-1, so that current position has y==target_len; i==0 so d=0 and y=s/2
        if (s / 2 >= target_len) {
            cur_score = extract_epi16(_m2, s / 2 - target_len);
            if (cur_score < minscore) {
                minscore = cur_score;
                minscoreidx = s + 1;
            }
        }

        _m2 = _mm_add_epi16(_m2, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin), _qualitieswin),
                                               _truthnqual));
        _d2 = _mm_min_epi16(_mm_add_epi16(_d1, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m1, _i1),  // allow I->D
                                          _gap_open));
        _i2 = _mm_insert_epi16(_mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_mm_srli_si128(_i1, 2), _gap_extend),
                                                           _mm_add_epi16(_mm_srli_si128(_m1, 2), _gap_open)),
                                             _nuc_prior),
                               inf, bandSize - 1);
        _backpointers[s + 1] = _mm_or_si128(_mm_or_si128(_mm_and_si128(_three, _m2),
                                                         _mm_slli_epi16(_mm_and_si128(_three, _i2), 2 * insertLabel)),
                                            _mm_slli_epi16(_mm_and_si128(_three, _d2), 2 * deleteLabel));

        // set state labels
        _m2 = _mm_andnot_si128(_three, _m2);
        _i2 = _mm_or_si128(_mm_andnot_si128(_three, _i2), _mm_srli_epi16(_three, 1));
        _d2 = _mm_or_si128(_mm_andnot_si128(_three, _d2), _three);
    }

    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 state = (reinterpret_cast<short*>(_backpointers.data() + s)[i] >> (2 * matchLabel)) & 3;

    s -= 2;

    // this is 2*y (s even) or 2*y+1 (s odd)
    while (y > 0) {
        const auto new_state = (reinterpret_cast<short*>(_backpointers.data() + s)[i] >> (2 * state)) & 3;

        if (state == matchLabel) {
            s -= 2;
            aln1[alnidx] = truth[--x];
            aln2[alnidx] = target[--y];
        } else if (state == insertLabel) {
            i += s & 1;
            s -= 1;
            aln1[alnidx] = gap;
            aln2[alnidx] = target[--y];
        } else {
            s -= 1;
            i -= s & 1;
            aln1[alnidx] = truth[--x];
            aln2[alnidx] = gap;
        }
        state = new_state;
        alnidx++;
    }

    aln1[alnidx] = 0;
    aln2[alnidx] = 0;

    first_pos = x;

    // reverse them
    for (int j {alnidx - 1}, i = 0; i < j; ++i, j--) {
        x = aln1[i];
        y = aln2[i];
        aln1[i] = aln1[j];
        aln2[i] = aln2[j];
        aln1[j] = x;
        aln2[j] = y;
    }

    return (minscore + 0x8000) >> 2;
}

int align(const char* truth, const char* target, const std::int8_t* qualities,
          int truth_len, int target_len,
          const char* snv_mask, const std::int8_t* snv_prior,
          const std::int8_t* gap_open, short gap_extend, short nuc_prior,
          char* aln1, char* aln2, int& first_pos) noexcept
{
    assert(truth_len > bandSize && (truth_len == target_len + 2 * bandSize - 1));
    assert(aln1 != nullptr && aln2 != nullptr);

    gap_extend <<= 2;
    nuc_prior <<= 2;

    constexpr int matchLabel  {0};
    constexpr int insertLabel {1};
    constexpr int deleteLabel {3};

    using SimdInt = __m128i;

    SimdInt _m1 {_mm_set1_epi16(inf)};
    auto _i1 = _m1;
    auto _d1 = _m1;
    auto _m2 = _m1;
    auto _i2 = _m1;
    auto _d2 = _m1;

    SimdInt _gap_extend {_mm_set1_epi16(gap_extend)};
    SimdInt _nuc_prior  {_mm_set1_epi16(nuc_prior)};
    SimdInt _initmask   {_mm_set_epi16(0,0,0,0,0,0,0,-1)};
    SimdInt _initmask2  {_mm_set_epi16(0,0,0,0,0,0,0,-0x8000)};

    static const SimdInt _three {_mm_set1_epi16(3)};
    SmallVector<SimdInt> _backpointers(2 * (truth_len + bandSize));

    SimdInt _truthwin  {_mm_set_epi16(truth[7], truth[6], truth[5], truth[4],
                                      truth[3], truth[2], truth[1], truth[0])};
    SimdInt _targetwin  {_m1};
    SimdInt _qualitieswin {_mm_set1_epi16(64 << 2)};

    SimdInt _snvmaskwin  {_mm_set_epi16(snv_mask[7], snv_mask[6], snv_mask[5], snv_mask[4],
                                        snv_mask[3], snv_mask[2], snv_mask[1], snv_mask[0])};
    SimdInt _snv_priorwin {_mm_set_epi16(snv_prior[7] << 2, snv_prior[6] << 2, snv_prior[5] << 2, snv_prior[4] << 2,
                                         snv_prior[3] << 2, snv_prior[2] << 2, snv_prior[1] << 2, snv_prior[0] << 2)};

    SimdInt _snvmask;

    // if N, make nScore; if != N, make inf
    SimdInt _truthnqual {_mm_add_epi16(_mm_and_si128(_mm_cmpeq_epi16(_truthwin, _mm_set1_epi16('N')),
                                                     _mm_set1_epi16(nScore - inf)),
                                       _mm_set1_epi16(inf))};
    SimdInt _gap_open {_mm_set_epi16(gap_open[7] << 2,gap_open[6] << 2,gap_open[5] << 2,gap_open[4] << 2,
                                     gap_open[3] << 2,gap_open[2] << 2,gap_open[1] << 2,gap_open[0] << 2)};

    short cur_score {0}, minscore {inf}, minscoreidx {-1};

    int s;
    for (s = 0; s <= 2 * (target_len + bandSize); s += 2) {
        // truth is current; target needs updating
        _targetwin    = _mm_slli_si128(_targetwin, 2);
        _qualitieswin = _mm_slli_si128(_qualitieswin, 2);

        if (s / 2 < target_len) {
            _targetwin    = _mm_insert_epi16(_targetwin, target[s / 2], 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, qualities[s / 2] << 2, 0);
        } else {
            _targetwin    = _mm_insert_epi16(_targetwin, '0', 0);
            _qualitieswin = _mm_insert_epi16(_qualitieswin, 64 << 2, 0);
        }

        // S even

        // initialize to -0x8000
        _m1 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m1));
        _m2 = _mm_or_si128(_initmask2, _mm_andnot_si128(_initmask, _m2));
        _m1 = _mm_min_epi16(_m1, _mm_min_epi16(_i1, _d1));

        // at this point, extract minimum score.  Referred-to position must
        // be y==target_len-1, so that current position has y==target_len; i==0 so d=0 and y=s/2

        if (s / 2 >= target_len) {
            cur_score = extract_epi16(_m1, s / 2 - target_len);
            if (cur_score < minscore) {
                minscore = cur_score;
                minscoreidx = s;     // point back to the match state at this entry, so as not to
            }                        // have to store the state at s-2
        }

        _snvmask = _mm_cmpeq_epi16(_targetwin, _snvmaskwin);
        _m1 = _mm_add_epi16(_m1, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _mm_min_epi16(_qualitieswin,
                                                                              _mm_or_si128(_mm_and_si128(_snvmask, _snv_priorwin),
                                                                                _mm_andnot_si128(_snvmask, _qualitieswin)))),
                                               _truthnqual));
        _d1 = _mm_min_epi16(_mm_add_epi16(_d2, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m2, _i2),
                                          _mm_srli_si128(_gap_open, 2))); // allow I->D
        _d1 = _mm_insert_epi16(_mm_slli_si128(_d1, 2), inf, 0);
        _i1 = _mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_i2, _gap_extend),
                                          _mm_add_epi16(_m2, _gap_open)), _nuc_prior);

        _backpointers[s] = _mm_or_si128(_mm_or_si128(_mm_and_si128(_three, _m1),
                                                     _mm_slli_epi16(_mm_and_si128(_three, _i1),
                                                                    2 * insertLabel)),
                                        _mm_slli_epi16(_mm_and_si128(_three, _d1), 2 * deleteLabel));

        // set state labels
        _m1 = _mm_andnot_si128(_three, _m1);
        _i1 = _mm_or_si128(_mm_andnot_si128(_three, _i1), _mm_srli_epi16(_three, 1));
        _d1 = _mm_or_si128(_mm_andnot_si128(_three, _d1), _three);

        // S odd

        // truth needs updating; target is current
        const auto pos = bandSize + s / 2;
        const char base {(pos < truth_len) ? truth[pos] : 'N'};

        _truthwin   = _mm_insert_epi16(_mm_srli_si128(_truthwin, 2), base, bandSize - 1);
        _truthnqual = _mm_insert_epi16(_mm_srli_si128(_truthnqual, 2),
                                       (base == 'N') ? nScore : inf, bandSize - 1);
        _snvmaskwin   = _mm_insert_epi16(_mm_srli_si128(_snvmaskwin, 2),
                                         pos < truth_len ? snv_mask[pos] : 'N', bandSize - 1);
        _snv_priorwin = _mm_insert_epi16(_mm_srli_si128(_snv_priorwin, 2),
                                         (pos < truth_len) ? snv_prior[pos] << 2 : inf << 2,
                                         bandSize - 1);
        _gap_open  = _mm_insert_epi16(_mm_srli_si128(_gap_open,  2),
                                      gap_open[pos < truth_len ? pos : truth_len - 1] << 2,
                                      bandSize - 1);

        _initmask  = _mm_slli_si128(_initmask, 2);
        _initmask2 = _mm_slli_si128(_initmask2, 2);

        _m2 = _mm_min_epi16(_m2, _mm_min_epi16(_i2, _d2));

        // at this point, extract minimum score.  Referred-to position must
        // be y==target_len-1, so that current position has y==target_len; i==0 so d=0 and y=s/2
        if (s / 2 >= target_len) {
            cur_score = extract_epi16(_m2, s / 2 - target_len);
            if (cur_score < minscore) {
                minscore = cur_score;
                minscoreidx = s + 1;
            }
        }

        _snvmask = _mm_cmpeq_epi16(_targetwin, _snvmaskwin);

        _m2 = _mm_add_epi16(_m2, _mm_min_epi16(_mm_andnot_si128(_mm_cmpeq_epi16(_targetwin, _truthwin),
                                                                _mm_min_epi16(_qualitieswin,
                                                                              _mm_or_si128(_mm_and_si128(_snvmask, _snv_priorwin),
                                                                                _mm_andnot_si128(_snvmask, _qualitieswin)))),
                                               _truthnqual));
        _d2 = _mm_min_epi16(_mm_add_epi16(_d1, _gap_extend),
                            _mm_add_epi16(_mm_min_epi16(_m1, _i1),  // allow I->D
                                          _gap_open));
        _i2 = _mm_insert_epi16(_mm_add_epi16(_mm_min_epi16(_mm_add_epi16(_mm_srli_si128(_i1, 2), _gap_extend),
                                                           _mm_add_epi16(_mm_srli_si128(_m1, 2), _gap_open)),
                                             _nuc_prior),
                               inf, bandSize - 1);
        _backpointers[s + 1] = _mm_or_si128(_mm_or_si128(_mm_and_si128(_three, _m2),
                                                         _mm_slli_epi16(_mm_and_si128(_three, _i2), 2 * insertLabel)),
                                            _mm_slli_epi16(_mm_and_si128(_three, _d2), 2 * deleteLabel));

        // set state labels
        _m2 = _mm_andnot_si128(_three, _m2);
        _i2 = _mm_or_si128(_mm_andnot_si128(_three, _i2), _mm_srli_epi16(_three, 1));
        _d2 = _mm_or_si128(_mm_andnot_si128(_three, _d2), _three);
    }

    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 state  = (reinterpret_cast<short*>(_backpointers.data() + s)[i] >> (2 * matchLabel)) & 3;

    s -= 2;

    // this is 2*y (s even) or 2*y+1 (s odd)
    while (y > 0) {
        const auto new_state = (reinterpret_cast<short*>(_backpointers.data() + s)[i] >> (2 * state)) & 3;

        if (state == matchLabel) {
            s -= 2;
            aln1[alnidx] = truth[--x];
            aln2[alnidx] = target[--y];
        } else if (state == insertLabel) {
            i += s & 1;
            s -= 1;
            aln1[alnidx] = gap;
            aln2[alnidx] = target[--y];
        } else {
            s -= 1;
            i -= s & 1;
            aln1[alnidx] = truth[--x];
            aln2[alnidx] = gap;
        }
        state = new_state;
        alnidx++;
    }

    aln1[alnidx] = 0;
    aln2[alnidx] = 0;
    first_pos = x;

    // reverse them
    for (int j {alnidx - 1}, i = 0; i < j; ++i, j--) {
        x = aln1[i];
        y = aln2[i];
        aln1[i] = aln1[j];
        aln2[i] = aln2[j];
        aln1[j] = x;
        aln2[j] = y;
    }

    return (minscore + 0x8000) >> 2;
}

int calculate_flank_score(const int truth_len, const int lhs_flank_len, const int rhs_flank_len,
                          const std::int8_t* quals, const std::int8_t* gap_open,
                          const short gap_extend, const short nuc_prior,
                          const int first_pos, const char* aln1, const char* aln2,
                          int& target_mask_size) noexcept
{
    static constexpr char match {'M'}, insertion {'I'}, deletion {'D'};

    auto prev_state = match;
    int x {first_pos}; // index into haplotype
    int y {0};         // index into read
    int i {0};         // index into alignment
    int result {0};    // alignment score (within flank)
    target_mask_size = 0;

    while (aln1[i]) {
        auto new_state = match;
        if (aln1[i] == gap) {
            new_state = insertion;
        } else if (aln2[i] == gap) { // can't be both '-'
            new_state = deletion;
        }
        switch (new_state) {
            case match:
            {
                if (x < lhs_flank_len || x >= (truth_len - rhs_flank_len)) {
                    if (aln1[i] != aln2[i]) {
                        if (aln1[i] != 'N') {
                            result += quals[y];
                        } else {
                            result += nScore >> 2;
                        }
                    }
                    ++target_mask_size;
                }
                ++x;
                ++y;
                break;
            }
            case insertion:
            {
                if (x < lhs_flank_len || x >= (truth_len - rhs_flank_len)) {
                    if (prev_state == insertion) {
                        result += gap_extend + nuc_prior;
                    } else {
                        // gap open score is charged for insertions just after the corresponding base, hence the -1
                        result += gap_open[x - 1] + nuc_prior;
                    }
                    ++target_mask_size;
                }
                ++y;
                break;
            }
            case deletion:
            {
                if (x < lhs_flank_len || x >= (truth_len - rhs_flank_len)) {
                    if (prev_state == deletion) {
                        result += gap_extend;
                    } else {
                        result += gap_open[x];
                    }
                }
                ++x;
                break;
            }
        }

        ++i;
        prev_state = new_state;
    }

    return result;
}

int calculate_flank_score(const int truth_len, const int lhs_flank_len, const int rhs_flank_len,
                          const char* target, const std::int8_t* quals,
                          const char* snv_mask, const std::int8_t* snv_prior,
                          const std::int8_t* gap_open, const short gap_extend, const short nuc_prior,
                          const int first_pos, const char* aln1, const char* aln2,
                          int& target_mask_size) noexcept
{
    static constexpr char match {'M'}, insertion {'I'}, deletion {'D'};

    auto prev_state = match;
    int x {first_pos}; // index into truth
    int y {0};         // index into target
    int i {0};         // index into alignment
    int result {0};    // alignment score (within flank)
    const auto rhs_flank_begin = truth_len - rhs_flank_len;
    target_mask_size = 0;

    while (aln1[i]) {
        auto new_state = match;
        if (aln1[i] == gap) {
            new_state = insertion;
        } else if (aln2[i] == gap) { // can't be both '-'
            new_state = deletion;
        }
        switch (new_state) {
            case match:
            {
                if (x < lhs_flank_len || x >= rhs_flank_begin) {
                    if (aln1[i] != aln2[i]) {
                        if (aln1[i] != 'N') {
                            result += (snv_mask[x] == target[y]) ? std::min(quals[y], snv_prior[x]) : quals[y];
                        } else {
                            result += nScore >> 2;
                        }
                    }
                    ++target_mask_size;
                }
                ++x;
                ++y;
                break;
            }
            case insertion:
            {
                if (x < lhs_flank_len || x >= rhs_flank_begin) {
                    if (prev_state == insertion) {
                        result += gap_extend + nuc_prior;
                    } else {
                        // gap open score is charged for insertions just after the corresponding base, hence the -1
                        result += gap_open[x - 1] + nuc_prior;
                    }
                    ++target_mask_size;
                }
                ++y;
                break;
            }
            case deletion:
            {
                if (x < lhs_flank_len || x >= rhs_flank_begin) {
                    if (prev_state == deletion) {
                        result += gap_extend;
                    } else {
                        result += gap_open[x];
                    }
                }
                ++x;
                break;
            }
        }
        ++i;
        prev_state = new_state;
    }

    return result;
}

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


// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

#ifndef SLHA_IO_H
#define SLHA_IO_H

#include <string>
#include <sstream>
#include <iosfwd>
#include <vector>
#include <Eigen/Core>
#include <boost/format.hpp>
#include <boost/function.hpp>
#include "slhaea.h"
#include "config.h"
#include "logger.hpp"
#include "error.hpp"
#include "wrappers.hpp"
#include "numerics2.hpp"
#include "pmns.hpp"
#include "standard_model_two_scale_model.hpp"

namespace softsusy {
   class QedQcd;
} // namespace softsusy

namespace flexiblesusy {

   class Spectrum_generator_settings;
   class Physical_input;

   namespace {
      /// SLHA line formatter for the MASS block entries
      const boost::format mass_formatter(" %9d   %16.8E   # %s\n");
      /// SLHA line formatter for the mixing matrix entries (NMIX, UMIX, VMIX, ...)
      const boost::format mixing_matrix_formatter(" %2d %2d   %16.8E   # %s\n");
      /// SLHA line formatter for vector entries
      const boost::format vector_formatter(" %5d   %16.8E   # %s\n");
      /// SLHA number formatter
      const boost::format number_formatter("         %16.8E   # %s\n");
      /// SLHA line formatter for entries with three indices
      const boost::format tensor_formatter(" %8d %8d %8d   %16.8E   # %s\n");
      /// SLHA scale formatter
      const boost::format scale_formatter("%9.8E");
      /// SLHA line formatter for the one-element entries (HMIX, GAUGE, MSOFT, ...)
      const boost::format single_element_formatter(" %5d   %16.8E   # %s\n");
      /// SLHA line formatter for the SPINFO block entries
      const boost::format spinfo_formatter(" %5d   %s\n");
   } // namespace

#define FORMAT_MASS(pdg,mass,name)                                      \
   boost::format(mass_formatter) % (pdg) % (mass) % (name)
#define FORMAT_MIXING_MATRIX(i,k,entry,name)                            \
   boost::format(mixing_matrix_formatter) % (i) % (k) % (entry) % (name)
#define FORMAT_ELEMENT(pdg,value,name)                                  \
   boost::format(single_element_formatter) % (pdg) % (value) % (name)
#define FORMAT_SCALE(n)                                                 \
   boost::format(scale_formatter) % (n)
#define FORMAT_NUMBER(n,str)                                            \
   boost::format(number_formatter) % (n) % (str)
#define FORMAT_SPINFO(n,str)                                            \
   boost::format(spinfo_formatter) % (n) % (str)
#define FORMAT_RANK_THREE_TENSOR(i,j,k,entry,name)                      \
   boost::format(tensor_formatter) % (i) % (j) % (k) % (entry) % (name)

/**
 * @class SLHA_io
 * @brief Handles reading and writing of SLHA files
 *
 * Reading: There are two ways to read block entries from SLHA files:
 * a) using the read_block() function with a %SLHA_io::Tuple_processor
 * or b) using the read_entry() function for each entry.  Note, that
 * a) is much faster than b) (more than 1000 times) because b) needs
 * to search for the block each time read_entry() is called.
 *
 * Example how to use a tuple processor (fast!):
 * \code{.cpp}
void process_tuple(double* array, int key, double value) {
   array[key] = value;
}

void read_file() {
   double array[1000];

   SLHA_io reader;
   reader.read_from_file("file.slha");

   SLHA_io::Tuple_processor processor = [&array] (int key, double value) {
      return process_tuple(array, key, value);
   };

   reader.read_block("MyBlock", processor);
}
 * \endcode
 *
 * Example how to use a for loop (slow!):
 * \code{.cpp}
void read_file() {
   double array[1000];

   SLHA_io reader;
   reader.read_from_file("file.slha");

   for (int i = 0; i < 1000; i++) {
      array[i] = reader.read_entry("MyBlock", i);
   }
}
 * \endcode
 */
class SLHA_io {
public:
   using Tuple_processor = std::function<void(int, double)>;
   enum Position { front, back };
   struct Modsel {
      bool quark_flavour_violated{false};   ///< MODSEL[6]
      bool lepton_flavour_violated{false};  ///< MODSEL[6]
      double parameter_output_scale{0.};    ///< MODSEL[12]
      void clear() { *this = Modsel(); }
   };

   struct CKM_wolfenstein {
      double lambdaW{0.}, aCkm{0.}, rhobar{0.}, etabar{0.};
      void clear() { *this = CKM_wolfenstein(); }
   };

   void clear();

   // reading functions
   bool block_exists(const std::string&) const;
   void fill(softsusy::QedQcd&) const;
   void fill(Spectrum_generator_settings&) const;
   void fill(Physical_input&) const;
   const Modsel& get_modsel() const { return modsel; }
   const SLHAea::Coll& get_data() const { return data; }
   void read_from_file(const std::string&);
   void read_from_source(const std::string&);
   void read_from_stream(std::istream&);
   double read_block(const std::string&, const Tuple_processor&) const;
   template <class Derived>
   double read_block(const std::string&, Eigen::MatrixBase<Derived>&) const;
   double read_block(const std::string&, double&) const;
   double read_entry(const std::string&, int) const;
   double read_scale(const std::string&) const;

   // writing functions
   void set_data(const SLHAea::Coll& data_) { data = data_; }
   void set_block(const std::ostringstream&, Position position = back);
   void set_block(const std::string&, Position position = back);
   void set_blocks(const std::vector<std::string>&, Position position = back);
   void set_block(const std::string&, double, const std::string&, double scale = 0.);
   template<class Scalar, int M, int N>
   void set_block(const std::string&, const Eigen::Matrix<std::complex<Scalar>, M, N>&, const std::string&, double scale = 0.);
   template<class Scalar, int M>
   void set_block(const std::string&, const Eigen::Matrix<std::complex<Scalar>, M, 1>&, const std::string&, double scale = 0.);
   template<class Scalar, int M, int N>
   void set_block_imag(const std::string&, const Eigen::Matrix<std::complex<Scalar>, M, N>&, const std::string&, double scale = 0.);
   template<class Scalar, int M>
   void set_block_imag(const std::string&, const Eigen::Matrix<std::complex<Scalar>, M, 1>&, const std::string&, double scale = 0.);
   template <class Derived>
   void set_block(const std::string&, const Eigen::MatrixBase<Derived>&, const std::string&, double scale = 0.);
   template <class Derived>
   void set_block_imag(const std::string&, const Eigen::MatrixBase<Derived>&, const std::string&, double scale = 0.);
   void set_modsel(const Modsel&);
   void set_physical_input(const Physical_input&);
   void set_settings(const Spectrum_generator_settings&);
   void set_sminputs(const softsusy::QedQcd&);
   void write_to_file(const std::string&) const;
   void write_to_stream(std::ostream& = std::cerr) const;

   // Standard_model class interface
   void set_mass(const standard_model::Standard_model_physical&);
   void set_mixing_matrices(const standard_model::Standard_model_physical&);
   void set_model_parameters(const standard_model::Standard_model&);
   void set_spectrum(const standard_model::Standard_model&);

   template<int N>
   static void convert_symmetric_fermion_mixings_to_slha(Eigen::Array<double, N, 1>&,
                                                         Eigen::Matrix<double, N, N>&);

   static void convert_symmetric_fermion_mixings_to_slha(double&,
                                                         Eigen::Matrix<double, 1, 1>&);

   template<int N>
   static void convert_symmetric_fermion_mixings_to_slha(Eigen::Array<double, N, 1>&,
                                                         Eigen::Matrix<std::complex<double>, N, N>&);

   static void convert_symmetric_fermion_mixings_to_slha(double&,
                                                         Eigen::Matrix<std::complex<double>, 1, 1>&);

   template<int N>
   static void convert_symmetric_fermion_mixings_to_hk(Eigen::Array<double, N, 1>&,
                                                       Eigen::Matrix<double, N, N>&);

   static void convert_symmetric_fermion_mixings_to_hk(double&,
                                                       Eigen::Matrix<double, 1, 1>&);

   template<int N>
   static void convert_symmetric_fermion_mixings_to_hk(Eigen::Array<double, N, 1>&,
                                                       Eigen::Matrix<std::complex<double>, N, N>&);

   static void convert_symmetric_fermion_mixings_to_hk(double&,
                                                       Eigen::Matrix<std::complex<double>, 1, 1>&);

private:
   SLHAea::Coll data{};        ///< SHLA data
   Modsel modsel{};            ///< data from block MODSEL
   template <class Scalar>
   static Scalar convert_to(const std::string&); ///< convert string
   static std::string to_lower(const std::string&); ///< string to lower case
   static void process_sminputs_tuple(softsusy::QedQcd&, int, double);
   static void process_modsel_tuple(Modsel&, int, double);
   static void process_vckmin_tuple(CKM_wolfenstein&, int, double);
   static void process_upmnsin_tuple(PMNS_parameters&, int, double);
   static void process_flexiblesusy_tuple(Spectrum_generator_settings&, int, double);
   static void process_flexiblesusyinput_tuple(Physical_input&, int, double);
   void read_modsel();
   template <class Derived>
   double read_matrix(const std::string&, Eigen::MatrixBase<Derived>&) const;
   template <class Derived>
   double read_vector(const std::string&, Eigen::MatrixBase<Derived>&) const;
};

template<class S>
struct Set_spectrum {
   S* slha_io;
   Set_spectrum(S* slha_io_) : slha_io(slha_io_) {}
   template<typename T>
   void operator()(const T& model) const { slha_io->set_spectrum(model); }
};

template <class Scalar>
Scalar SLHA_io::convert_to(const std::string& str)
{
   Scalar value;
   try {
      value = SLHAea::to<Scalar>(str);
   }  catch (const boost::bad_lexical_cast& error) {
      const std::string msg(R"(cannot convert string ")" + str + R"(" to )"
                            + typeid(Scalar).name());
      throw ReadError(msg);
   }
   return value;
}

/**
 * Fills a matrix from a SLHA block
 *
 * @param block_name block name
 * @param matrix matrix to be filled
 *
 * @return scale (or 0 if no scale is defined)
 */
template <class Derived>
double SLHA_io::read_matrix(const std::string& block_name, Eigen::MatrixBase<Derived>& matrix) const
{
   if (matrix.cols() <= 1) throw SetupError("Matrix has less than 2 columns");

   auto block = data.find(data.cbegin(), data.cend(), block_name);

   const int cols = matrix.cols(), rows = matrix.rows();
   double scale = 0.;

   while (block != data.cend()) {
      for (const auto& line: *block) {
         if (!line.is_data_line()) {
            // read scale from block definition
            if (line.size() > 3 &&
                to_lower(line[0]) == "block" && line[2] == "Q=")
               scale = convert_to<double>(line[3]);
            continue;
         }

         if (line.size() >= 3) {
            const int i = convert_to<int>(line[0]) - 1;
            const int k = convert_to<int>(line[1]) - 1;
            if (0 <= i && i < rows && 0 <= k && k < cols) {
               const double value = convert_to<double>(line[2]);
               matrix(i,k) = value;
            }
         }
      }

      ++block;
      block = data.find(block, data.cend(), block_name);
   }

   return scale;
}

/**
 * Fills a vector from a SLHA block
 *
 * @param block_name block name
 * @param vector vector to be filled
 *
 * @return scale (or 0 if no scale is defined)
 */
template <class Derived>
double SLHA_io::read_vector(const std::string& block_name, Eigen::MatrixBase<Derived>& vector) const
{
   if (vector.cols() != 1) throw SetupError("Vector has more than 1 column");

   auto block = data.find(data.cbegin(), data.cend(), block_name);

   const int rows = vector.rows();
   double scale = 0.;

   while (block != data.cend()) {
      for (const auto& line: *block) {
         if (!line.is_data_line()) {
            // read scale from block definition
            if (line.size() > 3 &&
                to_lower(line[0]) == "block" && line[2] == "Q=")
               scale = convert_to<double>(line[3]);
            continue;
         }

         if (line.size() >= 2) {
            const int i = convert_to<int>(line[0]) - 1;
            if (0 <= i && i < rows) {
               const double value = convert_to<double>(line[1]);
               vector(i,0) = value;
            }
         }
      }

      ++block;
      block = data.find(block, data.cend(), block_name);
   }

   return scale;
}

/**
 * Fills a matrix or vector from a SLHA block
 *
 * @param block_name block name
 * @param dense matrix or vector to be filled
 *
 * @return scale (or 0 if no scale is defined)
 */
template <class Derived>
double SLHA_io::read_block(const std::string& block_name, Eigen::MatrixBase<Derived>& dense) const
{
   return dense.cols() == 1
      ? read_vector(block_name, dense)
      : read_matrix(block_name, dense);
}

template<class Scalar, int NRows>
void SLHA_io::set_block(const std::string& name,
                        const Eigen::Matrix<std::complex<Scalar>, NRows, 1>& matrix,
                        const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   for (int i = 1; i <= NRows; ++i) {
      ss << boost::format(vector_formatter) % i % Re(matrix(i-1,0))
         % ("Re(" + symbol + "(" + ToString(i) + "))");
   }

   set_block(ss);
}

template<class Scalar, int NRows, int NCols>
void SLHA_io::set_block(const std::string& name,
                        const Eigen::Matrix<std::complex<Scalar>, NRows, NCols>& matrix,
                        const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   for (int i = 1; i <= NRows; ++i) {
      for (int k = 1; k <= NCols; ++k) {
         ss << boost::format(mixing_matrix_formatter) % i % k
            % Re(matrix(i-1,k-1))
            % ("Re(" + symbol + "(" + ToString(i) + ","
               + ToString(k) + "))");
      }
   }

   set_block(ss);
}

template<class Scalar, int NRows>
void SLHA_io::set_block_imag(const std::string& name,
                             const Eigen::Matrix<std::complex<Scalar>, NRows, 1>& matrix,
                             const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   for (int i = 1; i <= NRows; ++i) {
      ss << boost::format(vector_formatter) % i % Im(matrix(i-1,0))
         % ("Im(" + symbol + "(" + ToString(i) + "))");
   }

   set_block(ss);
}

template<class Scalar, int NRows, int NCols>
void SLHA_io::set_block_imag(const std::string& name,
                             const Eigen::Matrix<std::complex<Scalar>, NRows, NCols>& matrix,
                             const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   for (int i = 1; i <= NRows; ++i) {
      for (int k = 1; k <= NCols; ++k) {
         ss << boost::format(mixing_matrix_formatter) % i % k
            % Im(matrix(i-1,k-1))
            % ("Im(" + symbol + "(" + ToString(i) + ","
               + ToString(k) + "))");
      }
   }

   set_block(ss);
}

template <class Derived>
void SLHA_io::set_block(const std::string& name,
                        const Eigen::MatrixBase<Derived>& matrix,
                        const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   const int rows = matrix.rows();
   const int cols = matrix.cols();
   for (int i = 1; i <= rows; ++i) {
      if (cols == 1) {
         ss << boost::format(vector_formatter) % i % matrix(i-1,0)
            % (symbol + "(" + ToString(i) + ")");
      } else {
         for (int k = 1; k <= cols; ++k) {
            ss << boost::format(mixing_matrix_formatter) % i % k % matrix(i-1,k-1)
               % (symbol + "(" + ToString(i) + "," + ToString(k) + ")");
         }
      }
   }

   set_block(ss);
}

template <class Derived>
void SLHA_io::set_block_imag(const std::string& name,
                             const Eigen::MatrixBase<Derived>& matrix,
                             const std::string& symbol, double scale)
{
   std::ostringstream ss;
   ss << "Block " << name;
   if (scale != 0.)
      ss << " Q= " << FORMAT_SCALE(scale);
   ss << '\n';

   const int rows = matrix.rows();
   const int cols = matrix.cols();
   for (int i = 1; i <= rows; ++i) {
      if (cols == 1) {
         ss << boost::format(vector_formatter) % i % Im(matrix(i-1,0))
            % ("Im(" + symbol + "(" + ToString(i) + "))");
      } else {
         for (int k = 1; k <= cols; ++k) {
            ss << boost::format(mixing_matrix_formatter) % i % k % Im(matrix(i-1,k-1))
               % ("Im(" + symbol + "(" + ToString(i) + "," + ToString(k) + "))");
         }
      }
   }

   set_block(ss);
}

template<int N>
void SLHA_io::convert_symmetric_fermion_mixings_to_slha(Eigen::Array<double, N, 1>&,
                                                        Eigen::Matrix<double, N, N>&)
{
}

/**
 * Converts the given vector of masses and the corresponding (complex)
 * mixing matrix to SLHA convention: Matrix rows with non-zero
 * imaginary parts are multiplied by i and the corresponding mass
 * eigenvalue is multiplied by -1.  As a result the mixing matrix will
 * be real and the mass eigenvalues might be positive or negative.  It
 * is assumed that these mixings result from diagonalizing a symmetric
 * fermion mass matrix in the convention of Haber and Kane,
 * Phys. Rept. 117 (1985) 75-263.  This conversion makes sense only if
 * the original symmetric mass matrix is real-valued.
 *
 * @param m vector of masses
 * @param z mixing matrix
 */
template<int N>
void SLHA_io::convert_symmetric_fermion_mixings_to_slha(Eigen::Array<double, N, 1>& m,
                                                        Eigen::Matrix<std::complex<double>, N, N>& z)
{
   for (int i = 0; i < N; i++) {
      // check if i'th row contains non-zero imaginary parts
      if (!is_zero(z.row(i).imag().cwiseAbs().maxCoeff())) {
         z.row(i) *= std::complex<double>(0.0,1.0);
         m(i) *= -1;
#ifdef ENABLE_DEBUG
         if (!is_zero(z.row(i).imag().cwiseAbs().maxCoeff())) {
            WARNING("Row " << i << " of the following fermion mixing matrix"
                    " contains entries which have non-zero real and imaginary"
                    " parts:\nZ = " << z);
         }
#endif
      }
   }
}

template<int N>
void SLHA_io::convert_symmetric_fermion_mixings_to_hk(Eigen::Array<double, N, 1>&,
                                                      Eigen::Matrix<double, N, N>&)
{
}

/**
 * Converts the given vector of masses and the corresponding (real)
 * mixing matrix to Haber-Kane convention (Phys. Rept. 117 (1985)
 * 75-263): Masses are positive and mixing matrices can be complex.
 *
 * @param m vector of masses
 * @param z mixing matrix
 */
template<int N>
void SLHA_io::convert_symmetric_fermion_mixings_to_hk(Eigen::Array<double, N, 1>& m,
                                                      Eigen::Matrix<std::complex<double>, N, N>& z)
{
   for (int i = 0; i < N; i++) {
      if (m(i) < 0.) {
         z.row(i) *= std::complex<double>(0.0,1.0);
         m(i) *= -1;
      }
   }
}

} // namespace flexiblesusy

#endif


/// @file
/// @copyright The code is licensed under the BSD License
///            <http://opensource.org/licenses/BSD-2-Clause>,
///            Copyright (c) 2012-2015 Alexandre Hamez.
/// @author Alexandre Hamez

#pragma once

#include <initializer_list>
#include <type_traits> // enable_if, is_same
#include <unordered_map>
#include <vector>

#include <boost/container/flat_map.hpp>
#include <boost/container/flat_set.hpp>

#include "sdd/internal_manager_fwd.hh"
#include "sdd/dd/check_compatibility.hh"
#include "sdd/dd/context_fwd.hh"
#include "sdd/dd/definition.hh"
#include "sdd/dd/nary.hh"
#include "sdd/dd/operations_fwd.hh"
#include "sdd/dd/square_union.hh"
#include "sdd/mem/linear_alloc.hh"
#include "sdd/util/hash.hh"
#include "sdd/values/empty.hh"
#include "sdd/values/values_traits.hh"

namespace sdd { namespace dd {

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief The sum operation in the cache.
template <typename C>
struct LIBSDD_ATTRIBUTE_PACKED sum_op_impl
{
  /// @brief The textual representation of the union operator.
  static constexpr char symbol = '+';

  /// @brief Perform the SDD union algorithm.
  ///
  /// It's a so-called 'n-ary' union in the sense that we don't create intermediary SDD.
  /// Also, a lot of tests permit to break loops as soon as possible.
  template <typename InputIterator, typename NodeType>
  static
  std::enable_if_t< std::is_same<NodeType, hierarchical_node<C>>::value
                    or not values::values_traits<typename C::Values>::fast_iterable
                  , SDD<C>>
  work(InputIterator begin, InputIterator end, context<C>& cxt)
  {
    using node_type = NodeType;
    using valuation_type = typename node_type::valuation_type;

    mem::rewinder _(cxt.arena());

    auto operands_cit = begin;
    const auto operands_end = end;

    // Get the first operand as a node, we need it to initialize the algorithm.
    const node_type& head = mem::variant_cast<node_type>(**operands_cit);

    // Type of the list of successors for a valuation, to be merged with the union operation
    // right before calling the square union.
    using sum_builder_type = sum_builder<C, SDD<C>>;

    /// @todo Use intrusive hash map to save on memory allocations?
    // List all the successors for each valuation in the final alpha.
    std::unordered_map< valuation_type
                      , sum_builder_type
                      , std::hash<valuation_type>
                      , std::equal_to<valuation_type>
                      , mem::linear_alloc<std::pair<const valuation_type, sum_builder_type>>
                      >
      res( head.size(), std::hash<valuation_type>(), std::equal_to<valuation_type>()
         , mem::linear_alloc<std::pair<const valuation_type, sum_builder_type>>(cxt.arena()));

    using pair_type = std::pair<valuation_type, sum_builder_type>;

    // Needed to temporarily store arcs erased from res and arcs from the alpha visited in
    // the loop (B).
    std::vector<pair_type, mem::linear_alloc<pair_type>>
      save(mem::linear_alloc<pair_type>(cxt.arena()));
    save.reserve(head.size());

    // Used in test (F).
    std::vector<pair_type, mem::linear_alloc<pair_type>>
      remainder(mem::linear_alloc<pair_type>(cxt.arena()));
    remainder.reserve(head.size());

    // Initialize res with the alpha of the first operand.
    for (const auto& arc : head)
    {
      sum_builder_type succs(cxt);
      succs.add(arc.successor());
      res.emplace(arc.valuation(), std::move(succs));
    }

    // (A).
    for (++operands_cit; operands_cit != operands_end; ++operands_cit)
    {
      // Throw a Top if operands are incompatible (different types or different variables).
      check_compatibility(*begin, *operands_cit);

      const auto res_end = res.end();

      const node_type& node = mem::variant_cast<node_type>(**operands_cit);
      const auto alpha_end = node.end();

      // (B). For each arc of the current operand.
      for (auto alpha_cit = node.begin(); alpha_cit != alpha_end; ++alpha_cit)
      {
        // The current valuation may be modified, we need a copy.
        auto current_val = alpha_cit->valuation();
        auto current_succ = alpha_cit->successor();

        // Initialize the start of the next search.
        auto res_cit = res.begin();

        // (C). While the current valuation is not empty, test it against arcs in res.
        while (not values::empty_values(current_val) and res_cit != res_end)
        {
          const valuation_type& res_val = res_cit->first;
          sum_builder_type& res_succs = res_cit->second;

          // (D).
          if (current_val == res_val) // Same valuations.
          {
            save.emplace_back(res_val, std::move(res_succs));
            save.back().second.add(std::move(current_succ));
            res_cit = res.erase(res_cit);
            // Avoid useless insertion or temporary variables.
            goto equality;
          }

          intersection_builder<C, valuation_type> inter_builder(cxt);
          inter_builder.add(current_val);
          inter_builder.add(res_val);
          const valuation_type inter = intersection(cxt, std::move(inter_builder));

          // (E). The current valuation and the current arc from res have a common part.
          if (not values::empty_values(inter))
          {
            save.emplace_back(inter, res_succs);
            save.back().second.add(current_succ);

            // (F).
            valuation_type diff = difference(cxt, res_val, inter);
            if (not values::empty_values(diff))
            {
              // (res_val - inter) can't be in intersection, but we need to keep it
              // for the next arcs of the current alpha. So we put it in a temporary storage.
              // It will be added back in res when we have finished with the current valuation.
              remainder.emplace_back(std::move(diff), std::move(res_succs));
            }

            // We won't need the current arc of res for the current val, we already have the
            // common part. Now, the current valuation has to be tested against the next arcs
            // of res.
            res_cit = res.erase(res_cit);

            // (G). The current valuation is completely included in the current arc of res.
            if (current_val == inter)
            {
              // We can move to the next arc of the current operand.
              break;
            }

            // Continue with what remains of val. If val is empty, the loop will stop at the
            // next iteration.
            current_val = difference(cxt, current_val, inter);
          }
          else // (H). Empty intersection, lookup for next possible common parts.
          {
            ++res_cit;
          }
        } // While we're not at the end of res and val is not empty.

        // (I). For val or a part of val (it could have been modified during the previous
        // loop), we didn't find an intersection with any arc of res.
        if (not values::empty_values(current_val))
        {
          sum_builder_type succs(cxt);
          succs.add(std::move(current_succ));
          save.emplace_back(std::move(current_val), std::move(succs));
        }

        // Both arcs had the same valuation.
        equality:;

        // Reinject all parts that were removed in (F).
        for (auto& rem : remainder)
        {
          res.emplace(std::move(rem.first), std::move(rem.second));
        }
        remainder.clear();

      } // For each arc of the current operand.

      // Reinject all parts that were removed from res (all parts that have a non-empty
      // intersection with the current alpha) and all parts of the current alpha that have an
      // empty intersection with all the parts of res.
      res.insert(save.begin(), save.end());

      // We still need save.
      save.clear();
    } // End of iteration on operands.

    square_union<C, valuation_type> su(cxt);
    su.reserve(res.size());
    for (auto& arc : res)
    {
      // construct an operand for the square union: (successors union) --> valuation
      su.add(sum(cxt, std::move(arc.second)), std::move(arc.first));
    }

    return SDD<C>(head.variable(), su());
  }

  /// @brief Linear union of flat SDDs whose valuation are "fast iterable".
  template <typename InputIterator, typename NodeType>
  static
  std::enable_if_t< std::is_same<NodeType, flat_node<C>>::value
                    and values::values_traits<typename C::Values>::fast_iterable
                  , SDD<C>>
  work(InputIterator begin, InputIterator end, context<C>& cxt)
  {
    const auto& variable = mem::variant_cast<flat_node<C>>(**begin).variable();

    mem::rewinder _(cxt.arena());

    using values_type      = typename C::Values;
    using values_builder   = typename values::values_traits<values_type>::builder;
    using value_type       = typename values_type::value_type;
    using sum_builder_type = sum_builder<C, SDD<C>>;
    boost::container::flat_map< value_type, sum_builder_type, std::less<value_type>
                              , mem::linear_alloc<std::pair<value_type, sum_builder_type>>>
      value_to_succ( std::less<value_type>()
                   , mem::linear_alloc<std::pair<value_type, sum_builder_type>>(cxt.arena()));
    value_to_succ.reserve(std::distance(begin, end) * 2);

    for (auto cit = begin; cit != end; ++cit)
    {
      check_compatibility(*begin, *cit);

      const auto& node = mem::variant_cast<flat_node<C>>(**cit);
      for (const auto& arc : node)
      {
        const SDD<C> succ = arc.successor();
        for (const auto& value : arc.valuation())
        {
          const auto search = value_to_succ.find(value);
          if (search == value_to_succ.end())
          {
            value_to_succ.emplace_hint(search, value, sum_builder_type(cxt, {succ}));
          }
          else
          {
            search->second.add(succ);
          }
        }
      }
    }

    // The following is almost like the square union except that we use a values_builder to
    // efficiently create the valuation of an arc (rather than using a union).
    boost::container::flat_map< SDD<C>, values_builder, std::less<SDD<C>>
                              , mem::linear_alloc<std::pair<SDD<C>, values_builder>>>
      succ_to_value( std::less<SDD<C>>()
                   , mem::linear_alloc<std::pair<SDD<C>, values_builder>>(cxt.arena()));
    succ_to_value.reserve(value_to_succ.size());
    for (auto& value_succs : value_to_succ)
    {
      const SDD<C> succ = sum(cxt, std::move(value_succs.second));
      const auto search = succ_to_value.find(succ);
      if (search == succ_to_value.end())
      {
        values_builder tmp;
        tmp.insert(value_succs.first);
        succ_to_value.emplace_hint(search, std::move(succ), std::move(tmp));
      }
      else
      {
        search->second.insert(std::move(value_succs.first));
      }
    }

    alpha_builder<C, values_type> alpha(cxt);
    alpha.reserve(succ_to_value.size());
    for (auto& succ_values : succ_to_value)
    {
      alpha.add(values_type(std::move(succ_values.second)), std::move(succ_values.first));
    }

    return SDD<C>(variable, std::move(alpha));
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief Add an arc to the operands of the sum operation.
struct sum_builder_policy
{
  template <typename Container, typename Valuation>
  void
  add(Container& set, Valuation&& operand)
  {
    if (not values::empty_values(operand))
    {
      set.insert(std::forward<Valuation>(operand));
    }
  }
};

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief The sum operation of a set of SDD.
/// @related sdd::SDD
template <typename C>
inline
SDD<C>
sum(context<C>& cxt, sum_builder<C, SDD<C>>&& builder)
{
  if (builder.empty())
  {
    return zero<C>();
  }
  else if (builder.size() == 1)
  {
    return *builder.begin();
  }
  return cxt.sum_cache()({builder});
}

/*------------------------------------------------------------------------------------------------*/

/// @internal
/// @brief The sum operation of a set of values.
/// @details A wrapper around the implementation of sum provided by Values.
template <typename C, typename Values>
inline
Values
sum(context<C>&, sum_builder<C, Values>&& builder)
{
  if (builder.empty())
  {
    return Values();
  }
  else if (builder.size() == 1)
  {
    return *builder.begin();
  }
  else
  {
    auto cit = builder.begin();
    const auto end = builder.end();
    Values result = *cit;
    for (++cit; cit != end; ++cit)
    {
      typename C::Values tmp = sum(result, *cit);
      using std::swap;
      swap(tmp, result);
    }
    return result;
  }
}

} // namespace dd

/*------------------------------------------------------------------------------------------------*/

/// @brief Perform the union of two SDD.
/// @related SDD
template <typename C>
inline
SDD<C>
operator+(const SDD<C>& lhs, const SDD<C>& rhs)
{
  return dd::sum( global<C>().sdd_context
                , dd::sum_builder<C, SDD<C>>(global<C>().sdd_context, {lhs, rhs}));
}

/// @brief Perform the union of two SDD.
/// @related SDD
template <typename C>
inline
SDD<C>&
operator+=(SDD<C>& lhs, const SDD<C>& rhs)
{
  SDD<C> tmp = dd::sum( global<C>().sdd_context
                      , dd::sum_builder<C, SDD<C>>(global<C>().sdd_context, {lhs, rhs}));
  using std::swap;
  swap(tmp, lhs);
  return lhs;
}

/// @brief Perform the union of an iterable container of SDD.
/// @related SDD
template <typename C, typename InputIterator>
SDD<C>
inline
sum(InputIterator begin, InputIterator end)
{
  dd::sum_builder<C, SDD<C>> builder(global<C>().sdd_context);
  for (; begin != end; ++begin)
  {
    builder.add(*begin);
  }
  return dd::sum(global<C>().sdd_context, std::move(builder));
}

/// @brief Perform the union of an initializer list of SDD.
/// @related SDD
template <typename C>
SDD<C>
inline
sum(std::initializer_list<SDD<C>> operands)
{
  return sum<C>(std::begin(operands), std::end(operands));
}

/*------------------------------------------------------------------------------------------------*/

} // namespace sdd


/*
 * Copyright (c) 2011-2019, The DART development contributors
 * All rights reserved.
 *
 * The list of contributors can be found at:
 *   https://github.com/dartsim/dart/blob/master/LICENSE
 *
 * This file is provided under the following "BSD-style" License:
 *   Redistribution and use in source and binary forms, with or
 *   without modification, are permitted provided that the following
 *   conditions are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
 *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
 *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
 *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
 *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
 *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *   POSSIBILITY OF SUCH DAMAGE.
 */

#ifndef DART_OPTIMIZER_FUNCTION_HPP_
#define DART_OPTIMIZER_FUNCTION_HPP_

#include <functional>
#include <memory>
#include <string>

#include <Eigen/Dense>

namespace dart {
namespace optimizer {

class Function
{
public:
  /// Constructor
  explicit Function(const std::string& name = "function");

  /// Destructor
  virtual ~Function();

  /// Sets the name of this Function
  virtual void setName(const std::string& newName);

  /// Returns the name of this Function
  const std::string& getName() const;

  /// Evaluates and returns the objective function at the point x
  virtual double eval(const Eigen::VectorXd& x) const = 0;

  /// Evaluates and returns the objective function at the point x
  virtual void evalGradient(const Eigen::VectorXd& x,
                            Eigen::Map<Eigen::VectorXd> grad) const;

  /// Evaluates and return the objective function at the point x.
  ///
  /// If you have a raw array that the gradient will be passed in, then use
  /// evalGradient(const Eigen::VectorXd&, Eigen::Map<Eigen::VectorXd>)
  /// for better performance.
  void evalGradient(const Eigen::VectorXd& x, Eigen::VectorXd& grad) const;

  /// Evaluates and return the objective function at the point x
  virtual void evalHessian(
      const Eigen::VectorXd& x,
      Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> Hess) const;

protected:
  /// Name of this function
  std::string mName;
};

using FunctionPtr = std::shared_ptr<Function>;
using UniqueFunctionPtr = std::unique_ptr<Function>;

using CostFunction = std::function<double(const Eigen::VectorXd&)>;

using GradientFunction = std::function<void(
    const Eigen::VectorXd&, Eigen::Map<Eigen::VectorXd>)>;

using HessianFunction = std::function<void(
    const Eigen::VectorXd&,
    Eigen::Map<Eigen::VectorXd, Eigen::RowMajor>)>;

/// ModularFunction uses C++11 std::function to allow you to easily swap
/// out the cost function, gradient function, and Hessian function during
/// runtime for an optimizer::Function instance.
class ModularFunction : public Function
{
public:
  /// Constructor
  explicit ModularFunction(const std::string& name = "modular_function");

  /// Destructor
  ~ModularFunction() override;

  /// eval() will now call whatever CostFunction you set using
  /// setCostFunction()
  double eval(const Eigen::VectorXd& x) const override;

  /// evalGradient() will now call whatever GradientFunction you set
  /// using setGradientFunction()
  void evalGradient(const Eigen::VectorXd& x,
                    Eigen::Map<Eigen::VectorXd> grad) const override;

  /// evalHessian() will now call whatever HessianFunction you set using
  /// setHessianFunction()
  void evalHessian(
      const Eigen::VectorXd& x,
      Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> Hess) const override;

  /// Set the function that gets called by eval()
  void setCostFunction(CostFunction cost);

  /// Replace the cost function with a constant-zero function. Passing in
  /// true will cause a warning to be printed out whenever eval() is called.
  void clearCostFunction(bool printWarning = true);

  /// Set the function that gets called by evalGradient()
  void setGradientFunction(GradientFunction gradient);

  /// Replace the gradient function with the default evalGradient() of
  /// the base Function class. A warning will be printed whenever evalGradient()
  /// gets called.
  void clearGradientFunction();

  /// Set the function that gets called by evalHessian()
  void setHessianFunction(HessianFunction hessian);

  /// Replace the Hessian function with the default evalHessian() of the
  /// base Function class. A warning will be printed whenever evalHessian() gets
  /// called.
  void clearHessianFunction();

protected:
  /// Storage for the cost function
  CostFunction mCostFunction;

  /// Storage for the gradient function
  GradientFunction mGradientFunction;

  /// Storage for the Hessian function
  HessianFunction mHessianFunction;
};

/// NullFunction is a constant-zero Function
class NullFunction : public Function
{
public:
  /// Constructor
  explicit NullFunction(const std::string& name = "null_function");

  /// Destructor
  ~NullFunction() override;

  /// eval() will always return exactly zero
  double eval(const Eigen::VectorXd&) const override;

  /// evalGradient will always set grad to a zero vector that
  /// matches the dimensionality of _x
  void evalGradient(const Eigen::VectorXd& x,
                    Eigen::Map<Eigen::VectorXd> grad) const override;

  /// evalHessian() will always set Hess to a zero matrix that matches
  /// the dimensionality of _x
  void evalHessian(
      const Eigen::VectorXd& _x,
      Eigen::Map<Eigen::VectorXd, Eigen::RowMajor> Hess) const override;
};

class MultiFunction
{
public:
  /// Constructor
  MultiFunction();

  /// Destructor
  virtual ~MultiFunction();

  /// Operator ()
  virtual void operator()(const Eigen::VectorXd& x,
                          Eigen::Map<Eigen::VectorXd>& f,
                          Eigen::Map<Eigen::MatrixXd>& grad) = 0;
};

}  // namespace optimizer
}  // namespace dart

#endif  // DART_OPTIMIZER_FUNCTION_HPP_