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

#ifndef read_stats_hpp
#define read_stats_hpp

#include <vector>
#include <iterator>
#include <utility>
#include <algorithm>
#include <numeric>
#include <type_traits>

#include <boost/range/algorithm.hpp>

#include "basics/aligned_read.hpp"
#include "containers/mappable_map.hpp"
#include "mappable_algorithms.hpp"
#include "maths.hpp"
#include "type_tricks.hpp"

namespace octopus {

namespace detail {

template <typename T>
constexpr bool is_aligned_read = std::is_same<std::decay_t<T>, AlignedRead>::value;

template <typename Container>
constexpr bool is_aligned_read_container = is_aligned_read<typename Container::value_type>;

struct IsForward
{
    bool operator()(const AlignedRead& read) const
    {
        return !read.is_marked_reverse_mapped();
    }
};

struct IsReverse
{
    bool operator()(const AlignedRead& read) const
    {
        return read.is_marked_reverse_mapped();
    }
};

struct IsMappingQualityZero
{
    bool operator()(const AlignedRead& read) const noexcept
    {
        return read.mapping_quality() == 0;
    }
};

template <typename Map>
bool all_empty(const Map& map) noexcept
{
    return std::all_of(std::cbegin(map), std::cend(map), [] (const auto& p) { return p.second.empty(); });
}

template <typename T>
bool has_coverage(const T& reads, NonMapTag)
{
    using octopus::has_coverage;
    return has_coverage(reads);
}

template <typename T>
bool has_coverage(const T& reads, const GenomicRegion& region, NonMapTag)
{
    using octopus::has_coverage;
    return has_coverage(reads, region);
}

template <typename T>
unsigned min_coverage(const T& reads, NonMapTag)
{
    using octopus::min_coverage;
    return min_coverage(reads);
}

template <typename T>
unsigned min_coverage(const T& reads, const GenomicRegion& region, NonMapTag)
{
    using octopus::min_coverage;
    return min_coverage(reads, region);
}

template <typename T>
unsigned max_coverage(const T& reads, NonMapTag)
{
    using octopus::max_coverage;
    return max_coverage(reads);
}

template <typename T>
unsigned max_coverage(const T& reads, const GenomicRegion& region, NonMapTag)
{
    using octopus::max_coverage;
    return max_coverage(reads, region);
}

template <typename T>
double mean_coverage(const T& reads, const GenomicRegion& region, NonMapTag)
{
    if (reads.empty() || is_empty(region)) return 0;
    return maths::mean(calculate_positional_coverage(reads, region));
}

template <typename T>
double mean_coverage(const T& reads, NonMapTag)
{
    if (reads.empty()) return 0;
    return mean_coverage(reads, encompassing_region(reads), NonMapTag {});
}

template <typename T>
double stdev_coverage(const T& reads, const GenomicRegion& region, NonMapTag)
{
    if (reads.empty() || is_empty(region)) return 0;
    return maths::stdev(calculate_positional_coverage(reads, region));
}

template <typename T>
double stdev_coverage(const T& reads, NonMapTag)
{
    if (reads.empty()) return 0;
    return stdev_coverage(reads, encompassing_region(reads), NonMapTag {});
}

template <typename T>
std::size_t count_reads(const T& reads, NonMapTag)
{
    return reads.size();
}

template <typename T>
std::size_t count_reads(const T& reads, const GenomicRegion& region, NonMapTag)
{
    return count_overlapped(reads, region);
}

template <typename T>
std::size_t count_forward(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    return std::count_if(std::cbegin(reads), std::cend(reads), IsForward {});
}

template <typename T>
std::size_t count_forward(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    return std::count_if(std::cbegin(overlapped), std::cend(overlapped), IsForward {});
}

template <typename T>
std::size_t count_reverse(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    return std::count_if(std::cbegin(reads), std::cend(reads), IsReverse {});
}

template <typename T>
std::size_t count_reverse(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    return std::count_if(std::cbegin(overlapped), std::cend(overlapped), IsReverse {});
}

template <typename T>
std::size_t count_base_pairs(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& read) {
                               return curr + static_cast<std::size_t>(sequence_size(read));
                           });
}

template <typename T>
std::size_t count_base_pairs(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    return std::accumulate(std::cbegin(overlapped), std::cend(overlapped), std::size_t {0},
                           [&region] (const auto curr, const auto& read) {
                               return curr + sequence_size(read, region);
                           });
}

template <typename T>
std::size_t count_forward_base_pairs(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& read) {
                               return curr + ((IsForward()(read)) ? static_cast<std::size_t>(sequence_size(read)) : 0);
                           });
}

template <typename T>
std::size_t count_forward_base_pairs(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    return std::accumulate(std::cbegin(overlapped), std::cend(overlapped), std::size_t {0},
                           [&region] (const auto curr, const auto& read) {
                               return curr + ((IsForward()(read)) ? static_cast<std::size_t>(sequence_size(read, region)) : 0);
                           });
}

template <typename T>
std::size_t count_reverse_base_pairs(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {},
                           [] (const auto curr, const auto& read) {
                               return curr + ((IsReverse()(read)) ? static_cast<std::size_t>(sequence_size(read)) : 0);
                           });
}

template <typename T>
std::size_t count_reverse_base_pairs(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    return std::accumulate(std::cbegin(overlapped), std::cend(overlapped), std::size_t {0},
                           [&region] (const auto curr, const auto& read) {
                               return curr + ((IsReverse()(read)) ? static_cast<std::size_t>(sequence_size(read, region)) : 0);
                           });
}

template <typename T>
std::size_t count_mapq_zero(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    return std::count_if(std::cbegin(reads), std::cend(reads), IsMappingQualityZero {});
}

template <typename T>
std::size_t count_mapq_zero(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    return std::count_if(std::cbegin(overlapped), std::cend(overlapped), IsMappingQualityZero {});
}

struct MappingQualityLess
{
    bool operator()(const AlignedRead& lhs, const AlignedRead& rhs) const noexcept
    {
        return lhs.mapping_quality() < rhs.mapping_quality();
    }
};

template <typename T>
auto min_mapping_quality(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    auto result_itr = std::min_element(std::cbegin(reads), std::cend(reads), MappingQualityLess {});
    return result_itr->mapping_quality();
}

template <typename T>
auto min_mapping_quality(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    auto result_itr = std::min_element(std::cbegin(overlapped), std::cend(overlapped), MappingQualityLess {});
    return result_itr->mapping_quality();
}

template <typename T>
auto max_mapping_quality(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    auto result_itr = std::max_element(std::cbegin(reads), std::cend(reads), MappingQualityLess {});
    return result_itr->mapping_quality();
}

template <typename T>
auto max_mapping_quality(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    auto result_itr = std::max_element(std::cbegin(overlapped), std::cend(overlapped), MappingQualityLess {});
    return result_itr->mapping_quality();
}

template <typename T>
double rmq_mapping_quality(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    std::vector<double> qualities(reads.size());
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(qualities),
                   [] (const auto& read) { return static_cast<double>(read.mapping_quality()); });
    return maths::rmq<double>(qualities);
}

template <typename T>
double rmq_mapping_quality(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    std::vector<double> qualities(size(overlapped));
    std::transform(std::cbegin(overlapped), std::cend(overlapped), std::begin(qualities),
                   [] (const auto& read) { return static_cast<double>(read.mapping_quality()); });
    return maths::rmq<double>(qualities);
}

template <typename T>
double rmq_base_quality(const T& reads, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    std::vector<double> qualities {};
    qualities.reserve(count_base_pairs(reads, NonMapTag {}));
    for (const auto& read : reads) {
        for (const auto quality : read.base_qualities()) {
            qualities.push_back(static_cast<double>(quality));
        }
    }
    return maths::rmq<double>(qualities);
}

template <typename T>
double rmq_base_quality(const T& reads, const GenomicRegion& region, NonMapTag)
{
    static_assert(is_aligned_read_container<T>, "T must be a container of AlignedReads");
    const auto overlapped = overlap_range(reads, region);
    std::vector<double> qualities {};
    qualities.reserve(count_base_pairs(reads, region, NonMapTag {}));
    std::for_each(std::cbegin(overlapped), std::cend(overlapped), [&qualities] (const auto& read) {
        for (const auto quality : read.base_qualities()) {
            qualities.push_back(static_cast<double>(quality));
        }
    });
    return maths::rmq<double>(qualities);
}

template <typename ReadMap>
bool has_coverage(const ReadMap& reads, MapTag)
{
    return std::any_of(std::cbegin(reads), std::cend(reads),
                       [] (const auto& sample_reads) {
                           return has_coverage(sample_reads.second, NonMapTag {});
                       });
}

template <typename ReadMap>
bool has_coverage(const ReadMap& reads, const GenomicRegion& region, MapTag)
{
    return std::any_of(std::cbegin(reads), std::cend(reads),
                       [&region] (const auto& sample_reads) {
                           return has_coverage(sample_reads.second, region, NonMapTag {});
                       });
}

template <typename ReadMap>
unsigned min_coverage(const ReadMap& reads, MapTag)
{
    if (reads.empty()) return 0;
    std::vector<unsigned> sample_min_coverages(reads.size(), 0);
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(sample_min_coverages),
                   [] (const auto& sample_reads) {
                       return min_coverage(sample_reads.second, NonMapTag {});
                   });
    return *std::min_element(cbegin(sample_min_coverages), std::cend(sample_min_coverages));
}

template <typename ReadMap>
unsigned min_coverage(const ReadMap& reads, const GenomicRegion& region, MapTag)
{
    if (reads.empty()) return 0;
    std::vector<unsigned> sample_min_coverages(reads.size(), 0);
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(sample_min_coverages),
                   [&region] (const auto& sample_reads) {
                       return min_coverage(sample_reads.second, region, NonMapTag {});
                   });
    return *std::min_element(cbegin(sample_min_coverages), std::cend(sample_min_coverages));
}

template <typename ReadMap>
unsigned max_coverage(const ReadMap& reads, MapTag)
{
    if (reads.empty()) return 0;
    std::vector<unsigned> sample_max_coverages(reads.size(), 0);
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(sample_max_coverages),
                   [] (const auto& sample_reads) {
                       return max_coverage(sample_reads.second, NonMapTag {});
                   });
    return *std::max_element(cbegin(sample_max_coverages), std::cend(sample_max_coverages));
}

template <typename ReadMap>
unsigned max_coverage(const ReadMap& reads, const GenomicRegion& region, MapTag)
{
    if (reads.empty()) return 0;
    std::vector<unsigned> sample_max_coverages(reads.size(), 0);
    std::transform(std::cbegin(reads), std::cend(reads), std::begin(sample_max_coverages),
                   [&region] (const auto& sample_reads) {
                       return max_coverage(sample_reads.second, region, NonMapTag {});
                   });
    return *std::max_element(std::cbegin(sample_max_coverages), std::cend(sample_max_coverages));
}

template <typename T>
auto sum_positional_coverages(const T& reads, const GenomicRegion& region)
{
    const auto num_bases = size(region);
    std::vector<std::size_t> result(num_bases);
    for (const auto& p : reads) {
        const auto sample_coverages = calculate_positional_coverage(p.second, region);
        assert(sample_coverages.size() == num_bases);
        for (std::size_t i {0}; i < num_bases; ++i) {
            result[i] += sample_coverages[i];
        }
    }
    return result;
}

template <typename T>
double mean_coverage(const T& reads, const GenomicRegion& region, MapTag)
{
    if (reads.empty() || is_empty_region(region)) return 0.0;
    if (reads.size() == 1) return mean_coverage(std::cbegin(reads)->second, region, NonMapTag {});
    return maths::mean(sum_positional_coverages(reads, region));
}

template <typename T>
double mean_coverage(const T& reads, MapTag)
{
    if (reads.empty() || all_empty(reads)) return 0.0;
    return mean_coverage(reads, encompassing_region(reads), MapTag {});
}

template <typename T>
double stdev_coverage(const T& reads, const GenomicRegion& region, MapTag)
{
    if (reads.empty() || is_empty_region(region)) return 0.0;
    if (reads.size() == 1) return stdev_coverage(std::cbegin(reads)->second, region, NonMapTag {});
    return maths::stdev(sum_positional_coverages(reads, region));
}

template <typename T>
double stdev_coverage(const T& reads, MapTag)
{
    if (reads.empty() || all_empty(reads)) return 0.0;
    return stdev_coverage(reads, encompassing_region(reads), MapTag {});
}

template <typename T>
std::size_t count_reads(const T& reads, MapTag)
{
    return maths::sum_sizes(reads);
}

template <typename T>
std::size_t count_reads(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_reads(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_forward(const T& reads, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& sample_reads) {
                               return curr + count_forward(sample_reads.second, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_forward(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_forward(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_reverse(const T& reads, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& sample_reads) {
                               return curr + count_reverse(sample_reads.second, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_reverse(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_reverse(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_base_pairs(const T& reads, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& sample_reads) {
                               return curr + count_base_pairs(sample_reads.second, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_base_pairs(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_base_pairs(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_forward_base_pairs(const T& reads, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& sample_reads) {
                               return curr + count_forward_base_pairs(sample_reads.second, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_forward_base_pairs(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_forward_base_pairs(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_reverse_base_pairs(const T& reads, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& sample_reads) {
                               return curr + count_reverse_base_pairs(sample_reads.second, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_reverse_base_pairs(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_reverse_base_pairs(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_mapq_zero(const T& reads, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [] (const auto curr, const auto& sample_reads) {
                               return curr + count_mapq_zero(sample_reads.second, NonMapTag {});
                           });
}

template <typename T>
std::size_t count_mapq_zero(const T& reads, const GenomicRegion& region, MapTag)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), std::size_t {0},
                           [&region] (const auto curr, const auto& sample_reads) {
                               return curr + count_mapq_zero(sample_reads.second, region, NonMapTag {});
                           });
}

template <typename T>
auto min_mapping_quality(const T& reads, MapTag)
{
    auto result = min_mapping_quality(std::cbegin(reads)->second, NonMapTag {});
    std::for_each(std::next(std::cbegin(reads)), std::cend(reads), [&] (const auto& p) {
        result = std::min(result, min_mapping_quality(p.second, NonMapTag {}));
    });
    return result;
}

template <typename T>
auto min_mapping_quality(const T& reads, const GenomicRegion& region, MapTag)
{
    auto result = min_mapping_quality(std::cbegin(reads)->second, region, NonMapTag {});
    std::for_each(std::next(std::cbegin(reads)), std::cend(reads), [&] (const auto& p) {
        result = std::min(result, min_mapping_quality(p.second, region, NonMapTag {}));
    });
    return result;
}

template <typename T>
auto max_mapping_quality(const T& reads, MapTag)
{
    auto result = max_mapping_quality(std::cbegin(reads)->second, NonMapTag {});
    std::for_each(std::next(std::cbegin(reads)), std::cend(reads), [&] (const auto& p) {
        result = std::max(result, max_mapping_quality(p.second, NonMapTag {}));
    });
    return result;
}

template <typename T>
auto max_mapping_quality(const T& reads, const GenomicRegion& region, MapTag)
{
    auto result = max_mapping_quality(std::cbegin(reads)->second, region, NonMapTag {});
    std::for_each(std::next(std::cbegin(reads)), std::cend(reads), [&] (const auto& p) {
        result = std::max(result, max_mapping_quality(p.second, region, NonMapTag {}));
    });
    return result;
}

template <typename T>
double rmq_mapping_quality(const T& reads, MapTag)
{
    std::vector<double> qualities {};
    qualities.reserve(maths::sum_sizes(reads));
    for (const auto& sample_reads : reads) {
        std::transform(std::cbegin(sample_reads.second), std::cend(sample_reads.second),
                       std::back_inserter(qualities), [] (const auto& read) {
                           return static_cast<double>(read.mapping_quality());
                       });
    }
    return maths::rmq<double>(qualities);
}

template <typename T>
double rmq_mapping_quality(const T& reads, const GenomicRegion& region, MapTag)
{
    std::vector<double> qualities {};
    qualities.reserve(maths::sum_sizes(reads));
    for (const auto& sample_reads : reads) {
        const auto overlapped = overlap_range(sample_reads.second, region);
        std::transform(std::cbegin(overlapped), std::cend(overlapped),
                       std::back_inserter(qualities), [] (const auto& read) {
                           return static_cast<double>(read.mapping_quality());
                       });
    }
    return maths::rmq<double>(qualities);
}

template <typename T>
double rmq_base_quality(const T& reads, MapTag)
{
    std::vector<double> qualities {};
    qualities.reserve(count_base_pairs(reads, MapTag {}));
    for (const auto& sample_reads : reads) {
        for (const auto& read : sample_reads.second) {
            for (const auto quality : read.base_qualities()) {
                qualities.push_back(static_cast<double>(quality));
            }
        }
    }
    return maths::rmq<double>(qualities);
}

template <typename T>
double rmq_base_quality(const T& reads, const GenomicRegion& region, MapTag)
{
    std::vector<double> qualities {};
    qualities.reserve(count_base_pairs(reads, region, MapTag {}));
    for (const auto& sample_reads : reads) {
        const auto overlapped = overlap_range(sample_reads.second, region);
        std::for_each(std::cbegin(overlapped), std::cend(overlapped),
                      [&qualities] (const auto& read) {
                          for (const auto quality : read.base_qualities()) {
                              qualities.push_back(static_cast<double>(quality));
                          }
                      });
    }
    return maths::rmq<double>(qualities);
}

} // namespace detail

template <typename T, typename = enable_if_map<T>>
bool has_coverage(const T& reads)
{
    return detail::has_coverage(reads, MapTag {});
}

template <typename T, typename = enable_if_map<T>>
bool has_coverage(const T& reads, const GenomicRegion& region)
{
    return detail::has_coverage(reads, region, MapTag {});
}

template <typename T, typename = enable_if_map<T>>
unsigned min_coverage(const T& reads)
{
    return detail::min_coverage(reads, MapTag {});
}

template <typename T, typename = enable_if_map<T>>
unsigned min_coverage(const T& reads, const GenomicRegion& region)
{
    return detail::min_coverage(reads, region, MapTag {});
}

template <typename T, typename = enable_if_map<T>>
unsigned max_coverage(const T& reads)
{
    return detail::max_coverage(reads, MapTag {});
}

template <typename T, typename = enable_if_map<T>>
unsigned max_coverage(const T& reads, const GenomicRegion& region)
{
    return detail::max_coverage(reads, region, MapTag {});
}

template <typename T>
double mean_coverage(const T& reads)
{
    return detail::mean_coverage(reads, MapTagType<T> {});
}

template <typename T>
double mean_coverage(const T& reads, const GenomicRegion& region)
{
    return detail::mean_coverage(reads, region, MapTagType<T> {});
}

template <typename T>
double stdev_coverage(const T& reads)
{
    return detail::stdev_coverage(reads, MapTagType<T> {});
}

template <typename T>
double stdev_coverage(const T& reads, const GenomicRegion& region)
{
    return detail::stdev_coverage(reads, region, MapTagType<T> {});
}

template <typename T>
std::size_t count_reads(const T& reads)
{
    return detail::count_reads(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_reads(const T& reads, const GenomicRegion& region)
{
    return detail::count_reads(reads, region, MapTagType<T> {});
}

template <typename T>
std::size_t count_forward(const T& reads)
{
    return detail::count_forward(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_forward(const T& reads, const GenomicRegion& region)
{
    return detail::count_forward(reads, region, MapTagType<T> {});
}

template <typename T>
std::size_t count_reverse(const T& reads)
{
    return detail::count_reverse(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_reverse(const T& reads, const GenomicRegion& region)
{
    return detail::count_reverse(reads, region, MapTagType<T> {});
}

template <typename T>
double strand_bias(const T& reads)
{
    const auto num_forward_reads = static_cast<double>(count_forward(reads));
    const auto num_reverse_reads = static_cast<double>(count_reverse(reads));
    const auto total             = num_forward_reads + num_reverse_reads;
    return (total > 0) ? (num_forward_reads / total) : 0.0;
}

template <typename T>
double strand_bias(const T& reads, const GenomicRegion& region)
{
    const auto num_forward_reads = static_cast<double>(count_forward(reads, region));
    const auto num_reverse_reads = static_cast<double>(count_reverse(reads, region));
    const auto total             = num_forward_reads + num_reverse_reads;
    return (total > 0) ? (num_forward_reads / total) : 0.0;
}

template <typename T>
std::size_t count_base_pairs(const T& reads)
{
    return detail::count_base_pairs(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_base_pairs(const T& reads, const GenomicRegion& region)
{
    return detail::count_base_pairs(reads, region, MapTagType<T> {});
}

template <typename T>
std::size_t count_forward_base_pairs(const T& reads)
{
    return detail::count_forward_base_pairs(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_forward_base_pairs(const T& reads, const GenomicRegion& region)
{
    return detail::count_forward_base_pairs(reads, region, MapTagType<T> {});
}

template <typename T>
std::size_t count_reverse_base_pairs(const T& reads)
{
    return detail::count_reverse_base_pairs(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_reverse_base_pairs(const T& reads, const GenomicRegion& region)
{
    return detail::count_reverse_base_pairs(reads, region, MapTagType<T> {});
}

template <typename T>
std::size_t count_mapq_zero(const T& reads)
{
    return detail::count_mapq_zero(reads, MapTagType<T> {});
}

template <typename T>
std::size_t count_mapq_zero(const T& reads, const GenomicRegion& region)
{
    return detail::count_mapq_zero(reads, region, MapTagType<T> {});
}

template <typename T>
auto min_mapping_quality(const T& reads)
{
    return detail::min_mapping_quality(reads, MapTagType<T> {});
}

template <typename T>
auto min_mapping_quality(const T& reads, const GenomicRegion& region)
{
    return detail::min_mapping_quality(reads, region, MapTagType<T> {});
}

template <typename T>
auto max_mapping_quality(const T& reads)
{
    return detail::max_mapping_quality(reads, MapTagType<T> {});
}

template <typename T>
auto max_mapping_quality(const T& reads, const GenomicRegion& region)
{
    return detail::max_mapping_quality(reads, region, MapTagType<T> {});
}

template <typename T>
double rmq_mapping_quality(const T& reads)
{
    return detail::rmq_mapping_quality(reads, MapTagType<T> {});
}

template <typename T>
double rmq_mapping_quality(const T& reads, const GenomicRegion& region)
{
    return detail::rmq_mapping_quality(reads, region, MapTagType<T> {});
}

template <typename T>
double rmq_base_quality(const T& reads)
{
    return detail::rmq_base_quality(reads, MapTagType<T> {});
}

template <typename T>
double rmq_base_quality(const T& reads, const GenomicRegion& region)
{
    return detail::rmq_base_quality(reads, region, MapTagType<T> {});
}

template <typename ReadMap>
std::size_t count_samples_with_coverage(const ReadMap& reads)
{
    return std::count_if(std::cbegin(reads), std::cend(reads),
                         [] (const auto& sample_reads) {
                             return has_coverage(sample_reads.second);
                         });
}

template <typename ReadMap>
std::size_t count_samples_with_coverage(const ReadMap& reads, const GenomicRegion& region)
{
    return std::count_if(std::cbegin(reads), std::cend(reads),
                         [&region] (const auto& sample_reads) {
                             return has_coverage(sample_reads.second, region);
                         });
}

template <typename ReadMap>
unsigned sum_min_coverages(const ReadMap& reads)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), 0,
                           [] (auto curr, const auto& sample_reads) {
                               return curr + min_coverage(sample_reads.second);
                           });
}

template <typename ReadMap>
unsigned sum_min_coverages(const ReadMap& reads, const GenomicRegion& region)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), 0,
                           [&region] (auto curr, const auto& sample_reads) {
                               return curr + min_coverage(sample_reads.second, region);
                           });
}

template <typename ReadMap>
unsigned sum_max_coverages(const ReadMap& reads)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), 0,
                           [] (auto curr, const auto& sample_reads) {
                               return curr + max_coverage(sample_reads.second);
                           });
}

template <typename ReadMap>
unsigned sum_max_coverages(const ReadMap& reads, const GenomicRegion& region)
{
    return std::accumulate(std::cbegin(reads), std::cend(reads), 0,
                           [&region] (auto curr, const auto& sample_reads) {
                               return curr + max_coverage(sample_reads.second, region);
                           });
}

template <typename ReadMap>
std::size_t max_sample_read_count(const ReadMap& reads)
{
    if (reads.empty()) return 0;
    return std::max_element(std::cbegin(reads), std::cend(reads),
                            [] (const auto& lhs, const auto& rhs) {
                                return lhs.second.size() < rhs.second.size();
                            })->second.size();
}

} // namespace octopus

#endif


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://github.com/mCRL2org/mCRL2/blob/master/COPYING
//
// 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_0.txt)
//
/// \file normalize_sorts_test.cpp
/// \brief Test for normalizing sorts.

#define BOOST_TEST_MODULE normalize_sorts_test
#include "mcrl2/data/data_specification.h"
#include "mcrl2/data/normalize_sorts.h"
#include "mcrl2/data/parse.h"
#include <algorithm>
#include <boost/test/included/unit_test_framework.hpp>
#include <iterator>
#include <set>
#include <vector>

using namespace mcrl2;
using namespace mcrl2::data;

void test_normalize_sorts()
{
  std::string DATASPEC =
    "sort Bit = struct e0 | e1;      \n"
    "     AbsBit = struct arbitrary; \n"
    "                                \n"
    "map  inv: Bit -> Bit;           \n"
    "     h: Bit -> AbsBit;          \n"
    "                                \n"
    "eqn  inv(e0)  =  e1;            \n"
    "     inv(e1)  =  e0;            \n"
    ;

  data_specification dataspec = parse_data_specification(DATASPEC);

  data::function_symbol f;
  f = parse_function_symbol("abseq : AbsBit # AbsBit -> Set(Bool)", DATASPEC);
  dataspec.add_mapping(f);
  f = parse_function_symbol("absinv : AbsBit -> Set(AbsBit)", DATASPEC);
  dataspec.add_mapping(f);

  data_equation_vector equations = dataspec.user_defined_equations();
  data::normalize_sorts(equations, dataspec);
}

// The test below checks whether the calculation of a confluent and terminating rewrite system for types using
// Knuth-Bendix completion is efficient. Aleksi Peltonen showed in the spring of 2018 that Knuth-Bendix completion
// was exponential, causing the example below not to terminate within reasonable time.
void test_apply_knuth_bendix_completion_on_sorts()
{
  std::string DATASPEC =
    "sort A_t = Nat; B_t = Nat; C_t = Nat; D_t = Nat; E_t = Nat; F_t = Nat; G_t = Nat; H_t = Nat; I_t = Nat; J_t=Nat; K_t=Nat; \n"
    "     L_t = Nat; M_t = Nat; N_t = Nat; O_t = Nat;\n"
    "     S_t = struct s( A:A_t, B:B_t, C:C_t, D:D_t, E:E_t, F:F_t, G:G_t, H:H_t, I:I_t, J:J_t, K:K_t, L:L_t, M:M_t, N:N_t, O:O_t); \n";

  data_specification dataspec = parse_data_specification(DATASPEC);

  data_equation_vector equations = dataspec.user_defined_equations();
  data::normalize_sorts(equations, dataspec);
}

// The specification below led to an infinite loop in January 2019 when applying Knuth-Bendix completion.
// The reason was a missing normalisation of the rhs of type equations, leading to a loop. front_doorstate was
// mapped to rear_doorstate and vice versa.
// The specification is not well typed, as there are constants with different types. This test is to
// check whether there is no loop, and typechecking never finishes.
void test_loop_free_knuth_bendix_completion()
{
  std::string DATASPEC =
    "sort front_doorstate = struct open | closed; \n"
    "     rear_doorstate  = struct open | closed;\n"
    ;

  try
  {
    data_specification dataspec = parse_data_specification(DATASPEC);
  }
  catch (mcrl2::runtime_error& e)
  {
    // This is ok. A runtime exception is expected.
    return;
  }
  BOOST_CHECK(false); // No exception is not ok.
}

BOOST_AUTO_TEST_CASE(test_main)
{
  test_normalize_sorts();
  test_apply_knuth_bendix_completion_on_sorts();
  test_loop_free_knuth_bendix_completion();
}


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

// Overload of uBLAS prod function with MKL/GSL implementations
#include <votca/tools/linalg.h>

#include <votca/xtp/qmapemachine.h>
#include <boost/format.hpp>
#include <boost/filesystem.hpp>
#include <votca/ctp/logger.h>

#include <votca/xtp/elements.h>
#include <votca/xtp/espfit.h>

using boost::format;

namespace votca { namespace xtp {

QMAPEMachine::QMAPEMachine(ctp::XJob *job, ctp::Ewald3DnD *cape,
	 Property *opt, string sfx, int nst)
   : _job(job), _cape(cape),_isConverged(false) {

	// CONVERGENCE THRESHOLDS
    string key = sfx + ".qmmmconvg";
            _crit_dR =opt->ifExistsReturnElseReturnDefault<double>(key + ".dR",0.01);//nm
            _crit_dQ =opt->ifExistsReturnElseReturnDefault<double>(key + ".dQ",0.01);//e
            _crit_dE_QM =opt->ifExistsReturnElseReturnDefault<double>(key + ".dQdE_QM",0.001);//eV
            _crit_dE_MM  =opt->ifExistsReturnElseReturnDefault<double>(key + ".dE_MM",_crit_dE_QM);//eV
            _maxIter=opt->ifExistsReturnElseReturnDefault<int>(key + ".max_iter",32);

	// TASKS
	key = sfx + ".tasks";
		_run_ape = opt->ifExistsReturnElseReturnDefault<bool>(key+".run_ape",true);
		_run_dft = opt->ifExistsReturnElseReturnDefault<bool>(key+".run_dft",true);
		_run_gwbse = opt->ifExistsReturnElseReturnDefault<bool>(key+".run_gwbse",true);

        if(_run_dft){
            key=sfx+".dft";
            string dft_xml = opt->ifExistsReturnElseThrowRuntimeError<string>(key + ".dftengine");
            load_property_from_xml(_dft_options, dft_xml.c_str());
            _externalgridaccuracy=opt->ifExistsReturnElseReturnDefault<string>(key+".externalgrid","medium");
        }
                if(_run_gwbse){
	// GWBSE CONFIG
        key = sfx + ".gwbse";
		string gwbse_xml = opt->ifExistsReturnElseThrowRuntimeError<string>(key + ".gwbse_options");
		load_property_from_xml(_gwbse_options, gwbse_xml.c_str());

		_state = opt->ifExistsReturnElseReturnDefault<int>(key+".state",1);
                std::vector<string> choices={ "singlet","triplet"  };
		_type  = opt->ifExistsAndinListReturnElseThrowRuntimeError<string>(key+".type",choices);

		key = sfx + ".gwbse.filter";
                if (opt->exists(key + ".oscillator_strength") && _type != "triplet") {
                    _has_osc_filter = true;
                    _osc_threshold = opt->get(key + ".oscillator_strength").as<double> ();
                }
                if (opt->exists(key + ".localisation")) {
                    _has_loc_filter = true;

                    string temp = opt->get(key + ".localisation").as<string> ();
                    Tokenizer tok_cleanup(temp, ", \n\t");
                    std::vector <std::string> strings_vec;
                    tok_cleanup.ToVector(strings_vec);
                    if (strings_vec.size()!=2){
                        throw runtime_error("qmmmachine: Fragment and localisation threshold are not separated");
                    }
                    if(strings_vec[0]=="a" || strings_vec[0]=="A"){
                        _localiseonA=true;
                    }else if(strings_vec[0]=="b" || strings_vec[0]=="B"){
                         _localiseonA=false;
                    }else{
                        throw runtime_error("qmmmachine: Fragment label not known, either A or B");
                    }
                    _loc_threshold=boost::lexical_cast<double>(strings_vec[1]);
                }

                if (opt->exists(key + ".charge_transfer")) {
                    _has_dQ_filter = true;
                    _dQ_threshold = opt->get(key + ".charge_transfer").as<double> ();
                }
                if(_has_dQ_filter && _has_loc_filter){
                    throw runtime_error("Cannot use localisation and charge_transfer filter at the same time.");
                }
                }
                return;
    }

QMAPEMachine::~QMAPEMachine() {

    std::vector<QMMIter*> ::iterator qit;
    for (qit = _iters.begin(); qit < _iters.end(); ++qit) {
        delete *qit;
    }
    _iters.clear();

    std::vector<ctp::PolarSeg*> ::iterator pit;
    for (pit = target_bg.begin(); pit < target_bg.end(); ++pit) {
        delete *pit;
    }
    target_bg.clear();

    for (pit = target_fg.begin(); pit < target_fg.end(); ++pit) {
        delete *pit;
    }
    target_fg.clear();
}

void QMAPEMachine::Evaluate(ctp::XJob *job) {

	// PREPARE JOB DIRECTORY
	string jobFolder = "qmapejob_" + boost::lexical_cast<string>(_job->getId())
					 + "_" + _job->getTag();
	bool created = boost::filesystem::create_directory(jobFolder);

	CTP_LOG(ctp::logINFO,*_log) << flush;
	if (created) {
		CTP_LOG(ctp::logINFO,*_log) << "Created directory " << jobFolder << flush;
        }

    CTP_LOG(ctp::logINFO,*_log)
       << format("... dR %1$1.4f dQ %2$1.4f QM %3$1.4f MM %4$1.4f IT %5$d")
       % _crit_dR % _crit_dQ % _crit_dE_QM % _crit_dE_MM % _maxIter << flush;

    // FIGURE OUT CHARGE + MULTIPLICITY
    double dQ = 0.0;
    for (unsigned i = 0; i < _job->getPolarTop()->QM0().size(); ++i) {
        dQ += _job->getPolarTop()->QM0()[i]->CalcTotQ();
    }
    int chrg = round(dQ);
    int spin = ( (chrg < 0) ? -chrg:chrg ) % 2 + 1;
    CTP_LOG(ctp::logINFO,*_log) << "... Q = " << chrg << ", 2S+1 = " << spin << flush;

    if(chrg!=0){
        throw runtime_error("Charged DFT calculations are not possible at the moment");
    }
    qminterface.setMultipoleSplitting(false,1);

    int iterCnt = 0;
    int iterMax = _maxIter;
    for ( ; iterCnt < iterMax; ++iterCnt) {
        CTP_LOG(ctp::logINFO,*_log) << "QMMM ITERATION:" << iterCnt+1<<" of "<<iterMax << flush;
        //bool code = Iterate(jobFolder, iterCnt);
        Iterate(jobFolder, iterCnt);
        if (hasConverged()) {
            CTP_LOG(ctp::logINFO,*_log) << "QMMM CONVERGED after:" << iterCnt+1<<" iterations."<< flush;

        break; }
    }

    if (iterCnt == iterMax-1 && !_isConverged) {
        CTP_LOG(ctp::logWARNING,*_log)
            << format("Not converged within %1$d iterations.") % iterMax;
    }

    return;
}

bool QMAPEMachine::Iterate(string jobFolder, int iterCnt) {

    // CREATE ITERATION OBJECT & SETUP RUN DIRECTORY
    QMMIter *thisIter = this->CreateNewIter();
    int iter = iterCnt;
    string runFolder = jobFolder + "/iter_" + boost::lexical_cast<string>(iter);

    CTP_LOG(ctp::logINFO,*_log) << flush;
    bool created = boost::filesystem::create_directory(runFolder);
    if (created)
        CTP_LOG(ctp::logDEBUG,*_log) << "Created directory " << runFolder << flush;
    else
        CTP_LOG(ctp::logWARNING,*_log) << "Could not create directory " << runFolder << flush;

    Orbitals orb_iter_input;

    qminterface.GenerateQMAtomsFromPolarSegs(_job->getPolarTop(), orb_iter_input);

    if (iterCnt == 0) {
    dftengine.Initialize(&_dft_options);
    dftengine.setLogger(_log);
    dftengine.ConfigureExternalGrid(_externalgridaccuracy);
    dftengine.Prepare(&orb_iter_input);
    SetupPolarSiteGrids(dftengine.getExternalGridpoints(),orb_iter_input.QMAtoms());
    }

    // COMPUTE POLARIZATION STATE WITH QM0(0)
    if (_run_ape) {
        if (iterCnt == 0) {
            _cape->ShowAgenda(_log);
            // Reset FGC, start from BGP state, apply FP fields (BG & FG)
            _cape->EvaluateInductionQMMM(true, true, true, true, true);
            // Add BG, do not add MM1 & QM0
            _cape->EvaluatePotential(target_bg, true, false, false);
        }
        // Do not add BG & QM0, add MM1
        _cape->EvaluatePotential(target_fg, false, true, false);
            }

    dftengine.setExternalGrid(ExtractElGrid_fromPolarsites(),ExtractNucGrid_fromPolarsites());

    if (_run_dft) {
    dftengine.Evaluate(&orb_iter_input);
    }
    FILE *out;
	out = fopen((runFolder + "/system.pdb").c_str(),"w");
	orb_iter_input.WritePDB(out,"Full structure");
	fclose(out);

    // Run GWBSE
	if (_run_gwbse){
		EvaluateGWBSE(orb_iter_input, runFolder);
	}

	// COMPUTE POLARIZATION STATE WITH QM0(n+1)
	if (_run_ape) {
		// Update QM0 density: QM0(n) => QM0(n+1)
		// ...
            std::vector< ctp::QMAtom* > Atomlist;
    for(const auto& atom:orb_iter_input.QMAtoms()){
                if(!atom->from_environment){
                Atomlist.push_back(atom);
                }
            }
        thisIter->UpdatePosChrgFromQMAtoms(Atomlist,_job->getPolarTop()->QM0());
		// Do not reset FGC (= only reset FU), do not use BGP state, nor apply FP fields (BG & FG)
        _cape->EvaluateInductionQMMM(false, false, false, false, false);
		// COMPUTE MM ENERGY
	_cape->EvaluateEnergyQMMM();
	_cape->ShowEnergySplitting(_log);
	}

	// COMPILE HAMILTONIAN & CHECK FOR CONVERGENCE
	// ...

    // THIS IS A SHORT VERSION OF PEWALD3D WITH QM/MM ENERGY SPLITTING
    _cape->ShowAgenda(_log);
    _cape->EvaluateInductionQMMM(true, true, true, true, true);
    _cape->EvaluateEnergyQMMM();
    _cape->ShowEnergySplitting(_log);

    return true;
}

QMMIter *QMAPEMachine::CreateNewIter() {

    QMMIter *newIter = new QMMIter(_iters.size());
    this->_iters.push_back(newIter);
    return newIter;
        }

bool QMAPEMachine::EvaluateGWBSE(Orbitals &orb, string runFolder) {

    std::vector<int> _state_index;
    GWBSE _gwbse(&orb);
    _gwbse.setLogger(_log);

    if (_state > 0) {
        CTP_LOG(ctp::logDEBUG, *_log) << "Excited state via GWBSE: " << flush;
        CTP_LOG(ctp::logDEBUG, *_log) << "  --- type:              " << _type << flush;
        CTP_LOG(ctp::logDEBUG, *_log) << "  --- state:             " << _state << flush;
        if (_has_osc_filter) {
            CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: osc.str. > " << _osc_threshold << flush;
        }
        if (_has_dQ_filter) {
            CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: crg.trs. > " << _dQ_threshold << flush;
        }
        if (_has_loc_filter){
            if (_loc_threshold>0.5){
             CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: localisation > " << _loc_threshold << flush;
            }else{
                CTP_LOG(ctp::logDEBUG, *_log) << "  --- filter: localisation < " << _loc_threshold << flush;
            }
        }

        if (_has_osc_filter && _has_dQ_filter) {
            CTP_LOG(ctp::logDEBUG, *_log) << "  --- WARNING: filtering for optically active CT transition - might not make sense... " << flush;
        }

        // define own logger for GW-BSE that is written into a runFolder logfile
        ctp::Logger gwbse_logger(ctp::logDEBUG);
        gwbse_logger.setMultithreading(false);
        //gwbse_logger.setPreface(ctp::logINFO, (format("\nGWBSE INF ...")).str());
        gwbse_logger.setPreface(ctp::logERROR, (format("\nGWBSE ERR ...")).str());
        gwbse_logger.setPreface(ctp::logWARNING, (format("\nGWBSE WAR ...")).str());
        gwbse_logger.setPreface(ctp::logDEBUG, (format("\nGWBSE DBG ...")).str());
        _gwbse.Initialize(&_gwbse_options);
        // actual GW-BSE run

        _gwbse.Evaluate();

        // write logger to log file
        ofstream ofs;
        string gwbse_logfile = runFolder + "/gwbse.log";
        ofs.open(gwbse_logfile.c_str(), ofstream::out);
        if (!ofs.is_open()) {
            throw runtime_error("Bad file handle: " + gwbse_logfile);
        }
        ofs << gwbse_logger << endl;
        ofs.close();

        // PROCESSING the GW-BSE result
        // - find the excited state of interest
        // oscillator strength filter

        if (_has_osc_filter) {

            // go through list of singlets
            const std::vector<double> oscs = orb.Oscillatorstrengths();
            for (unsigned _i = 0; _i < oscs.size(); _i++) {

                double osc =oscs[_i];
                if (osc > _osc_threshold) _state_index.push_back(_i);
            }
        } else {
            const ub::vector<real_gwbse>& energies = (_type=="singlet")
                        ? orb.BSESingletEnergies() : orb.BSETripletEnergies();

                        for (unsigned _i = 0; _i < energies.size(); _i++) {
                            _state_index.push_back(_i);
                        }
        }

       // filter according to charge transfer, go through list of excitations in _state_index
        if (_has_dQ_filter) {
            std::vector<int> _state_index_copy;
            const std::vector< ub::vector<double> >& dQ_frag= (_type=="singlet")
            ? orb.getFragmentChargesSingEXC():orb.getFragmentChargesTripEXC();
            for (unsigned _i = 0; _i < _state_index.size(); _i++) {
                if (std::abs(dQ_frag[_state_index[_i]](0)) > _dQ_threshold) {
                    _state_index_copy.push_back(_state_index[_i]);
                }
            }
            _state_index = _state_index_copy;
        }
        else if (_has_loc_filter) {
            std::vector<int> _state_index_copy;
            const std::vector< ub::vector<double> >& popE= (_type=="singlet")
            ? orb.getFragment_E_localisation_singlet():orb.getFragment_E_localisation_triplet();
            const std::vector< ub::vector<double> >& popH= (_type=="singlet")
            ? orb.getFragment_H_localisation_singlet():orb.getFragment_H_localisation_triplet();
            if(_localiseonA){
                for (unsigned _i = 0; _i < _state_index.size(); _i++) {
                    if (popE[_state_index[_i]](0) > _loc_threshold && popH[_state_index[_i]](0) > _loc_threshold ) {
                        _state_index_copy.push_back(_state_index[_i]);
                    }
                }
            }else{
                for (unsigned _i = 0; _i < _state_index.size(); _i++) {
                    if (popE[_state_index[_i]](1) > _loc_threshold && popH[_state_index[_i]](1) > _loc_threshold ) {
                        _state_index_copy.push_back(_state_index[_i]);
                    }
                }
            }
            _state_index = _state_index_copy;
        }
        if (_state_index.size() < 1) {
            CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " WARNING: FILTER yielded no state. Taking lowest excitation"<< flush;
            _state_index.push_back(0);
        }else{
                        CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Filter yielded state"<<_type<<":"<<_state_index[_state - 1]+1<< flush;
        }
    } // only if state >0

    std::vector< ctp::QMAtom* > Atomlist;
    for(const auto& atom:orb.QMAtoms()){
                if(!atom->from_environment){
                Atomlist.push_back(atom);
                }
            }

    // load DFT basis set (element-wise information) from xml file
    BasisSet dftbs;

    dftbs.LoadBasisSet(orb.getDFTbasis());

    CTP_LOG(ctp::logDEBUG, *_log) << ctp::TimeStamp() << " Loaded DFT Basis Set " << orb.getDFTbasis() << flush;

    // fill DFT AO basis by going through all atoms
    AOBasis dftbasis;
    dftbasis.AOBasisFill(&dftbs, Atomlist );
    // TBD: Need to switch between singlets and triplets depending on _type
    ub::matrix<double> DMATGS = orb.DensityMatrixGroundState();
    ub::matrix<double> DMAT_tot = DMATGS; // Ground state + hole_contribution + electron contribution

    if (_state > 0) {

        std::vector<ub::matrix<double> > DMAT = orb.DensityMatrixExcitedState(_type, _state_index[_state - 1]);
        DMAT_tot = DMAT_tot - DMAT[0] + DMAT[1]; // Ground state + hole_contribution + electron contribution
    }

    // fill DFT AO basis by going through all atoms

    Espfit esp = Espfit(_log);
    if (_run_gwbse) {
        esp.setUseECPs(true);
    }
    esp.Fit2Density(Atomlist, DMAT_tot, dftbasis, dftbs, "medium");
    return true;
}

void QMAPEMachine::SetupPolarSiteGrids(const std::vector<const vec *>& gridpoints,const std::vector<ctp::QMAtom*>& atoms){
    NumberofAtoms=0;
    std::vector<ctp::QMAtom*>::const_iterator qmt;
    std::vector<ctp::APolarSite*> sites1;
    std::vector<ctp::APolarSite*> sites2;

    for(qmt=atoms.begin();qmt!=atoms.end();++qmt){
        if((*qmt)->from_environment){continue;}
        NumberofAtoms++;
        sites1.push_back(qminterface.Convert(*qmt));
        sites2.push_back(qminterface.Convert(*qmt));
    }

    std::vector<const vec *>::const_iterator grt;
    for (grt=gridpoints.begin();grt<gridpoints.end();++grt){
        ctp::APolarSite* site1=new ctp::APolarSite();
        ctp::APolarSite* site2=new ctp::APolarSite();
        tools::vec pos=*(*grt)*tools::conv::bohr2nm;
        site1->setPos(pos);
        site2->setPos(pos);
        sites1.push_back(site1);
        sites2.push_back(site2);
    }

    target_bg.push_back( new ctp::PolarSeg(0, sites1));
    target_fg.push_back( new ctp::PolarSeg(0, sites2));

    return;
        }

std::vector<double> QMAPEMachine::ExtractNucGrid_fromPolarsites() {
    std::vector<double> gridpoints;
    double int2hrt = tools::conv::int2V * tools::conv::ev2hrt;
    for (unsigned i = 0; i < target_bg.size(); ++i) {
        for (unsigned j = 0; j < NumberofAtoms; ++j) {
            ctp::PolarSeg* seg1 = target_bg[i];
            ctp::PolarSeg* seg2 = target_fg[i];
            ctp::APolarSite* site1 = (*seg1)[j];
            ctp::APolarSite* site2 = (*seg2)[j];
            double value = (site1->getPhi() + site2->getPhi()) * int2hrt;
            gridpoints.push_back(value);
        }
    }
    return gridpoints;
}

std::vector<double> QMAPEMachine::ExtractElGrid_fromPolarsites() {
    std::vector<double> gridpoints;
    double int2hrt = tools::conv::int2V * tools::conv::ev2hrt;
    for (unsigned i = 0; i < target_bg.size(); ++i) {
        for (unsigned j = NumberofAtoms; j < target_bg[i]->size(); ++j) {
            ctp::PolarSeg* seg1 = target_bg[i];
            ctp::PolarSeg* seg2 = target_fg[i];
            ctp::APolarSite* site1 = (*seg1)[j];
            ctp::APolarSite* site2 = (*seg2)[j];
            double value = (site1->getPhi() + site2->getPhi()) * int2hrt;
            gridpoints.push_back(value);
        }
    }
    return gridpoints;
}

bool QMAPEMachine::hasConverged() {

    _convg_dR = false;
    _convg_dQ = false;
    _convg_dE_QM = false;
    _convg_dE_MM = false;

    if (_iters.size() > 1) {

        QMMIter *iter_0 = _iters[_iters.size()-2];
        QMMIter *iter_1 = _iters[_iters.size()-1];

        double dR = iter_1->getRMSdR();
        double dQ = iter_1->getRMSdQ();
        double dE_QM = iter_1->getQMEnergy() - iter_0->getQMEnergy();
        double dE_MM = iter_1->getMMEnergy() - iter_0->getMMEnergy();

        if (dR <= _crit_dR) _convg_dR = true;
        if (dQ <= _crit_dQ) _convg_dQ = true;
        if (dE_QM*dE_QM <= _crit_dE_QM*_crit_dE_QM) _convg_dE_QM = true;
        if (dE_MM*dE_MM <= _crit_dE_MM*_crit_dE_MM) _convg_dE_MM = true;
    }

    _isConverged = ((_convg_dR && _convg_dQ) && (_convg_dE_QM && _convg_dE_MM));

    CTP_LOG(ctp::logINFO,*_log)
        << (format("Convergence check")) << flush;
    CTP_LOG(ctp::logINFO,*_log)
        << format("  o Converged dR ? %s") % (_convg_dR ? "True" : "False") << flush;
    CTP_LOG(ctp::logINFO,*_log)
        << format("  o Converged dQ ? %s") % (_convg_dQ ? "True" : "False") << flush;
    CTP_LOG(ctp::logINFO,*_log)
        << format("  o Converged QM ? %s") % (_convg_dE_QM ? "True" : "False") << flush;
    CTP_LOG(ctp::logINFO,*_log)
        << format("  o Converged MM ? %s") % (_convg_dE_MM ? "True" : "False") << flush;

    return _isConverged;
}

}}


#include <cctbx/adptbx/hirshfeld.h>
#include <scitbx/boost_python/container_conversions.h>
#include <boost/python/class.hpp>
#include <boost/python/with_custodian_and_ward.hpp>
#include <boost/python/return_arg.hpp>
#include <boost/python/return_by_value.hpp>
#include <boost/python/return_value_policy.hpp>
#include <boost/python/make_function.hpp>

namespace cctbx { namespace adptbx { namespace boost_python {

  template <typename T>
  struct mean_square_displacement_wrapper
  {
    typedef mean_square_displacement<T> wt;

    static void wrap(char const *name) {
      using namespace boost::python;
      return_value_policy<return_by_value> rbv;
      class_<wt>(name, no_init)
        .def(init<uctbx::unit_cell const &,
                  scitbx::vec3<T> const &>
             ((arg("unit_cell"), arg("z")))
             [with_custodian_and_ward<1, 2>()])
        .def("__call__", &wt::operator(), return_self<>())
        .add_property("value", &wt::value)
        .add_property("grad_u",
                      make_function(&wt::grad_u, rbv))
        .add_property("grad_z",
                      make_function(&wt::grad_z, rbv))
        .add_property("grad_g",
                      make_function(&wt::grad_g, rbv))
        .add_property("grad_unit_cell_params",
                      make_function(&wt::grad_unit_cell_params, rbv))
        .add_property("well_defined", &wt::well_defined)
        ;
    }
  };

  template <typename T>
  struct relative_hirshfeld_difference_wrapper
  {
    typedef relative_hirshfeld_difference<T> wt;

    static void wrap(char const *name) {
      using namespace boost::python;
      using namespace scitbx::boost_python::container_conversions;
      return_value_policy<return_by_value> rbv;
      tuple_mapping_fixed_size<af::tiny<T, 2*(3+6)> >();
      class_<wt>(name, no_init)
      .def(init<uctbx::unit_cell const &,
                scitbx::vec3<T> const &,
                scitbx::sym_mat3<T> const &,
                scitbx::vec3<T> const &,
                scitbx::sym_mat3<T> const &,
                sgtbx::rt_mx const &>
           ((arg("unit_cell"),
             arg("site_1"), arg("u_star_1"),
             arg("site_2"), arg("u_star_2"), arg("rt_mx_2"))))
      .add_property("value", &wt::value)
      .add_property("grad_x1", make_function(&wt::grad_x1, rbv))
      .add_property("grad_x2", make_function(&wt::grad_x2, rbv))
      .add_property("grad_u1", make_function(&wt::grad_u1, rbv))
      .add_property("grad_u2", make_function(&wt::grad_u2, rbv))
      .add_property("grad_unit_cell_params",
                    make_function(&wt::grad_unit_cell_params, rbv))
      .def("esd", &wt::esd,
           (arg("crystallographic_variance_matrix_packed_u"),
            arg("index_x1"), arg("index_u1"), arg("index_x2"), arg("index_u2"),
            arg("a_b_c_alpha_beta_gamma_sigmas")))
      ;
    }
  };

  void wrap_hirshfeld() {
    mean_square_displacement_wrapper<double>
    ::wrap("mean_square_displacement");
    relative_hirshfeld_difference_wrapper<double>
    ::wrap("relative_hirshfeld_difference");
  }

}}}


#pragma once
//===----------------------------------------------------------------------===//
#include <dtl/bitmap/util/plain_bitmap.hpp>
#include <dtl/bitmap/util/plain_bitmap_iter.hpp>
#include <dtl/dtl.hpp>

#include <boost/dynamic_bitset.hpp>

#include <cstddef>
#include <type_traits>
#include <vector>
//===----------------------------------------------------------------------===//
namespace dtl {
//===----------------------------------------------------------------------===//
/// Byte-aligned Bitmap Code (BBC)
///
/// The implementation is based on the paper "Notes on Design and Implementation
/// of Compressed Bit Vectors" by Wu et al.
///
/// Note: The implementation is NOT yet optimized for performance.
class bbc {

protected:
  using byte_t = $u8;

  static constexpr byte_t all_zero = 0b00000000;
  static constexpr byte_t all_one = 0b11111111;

  /// The maximum number of fill bytes per header byte.
  static constexpr std::size_t max_fill_byte_cnt = 1u << (7 * 4);
  /// The maximum number of tail bytes per header byte.
  static constexpr std::size_t max_tail_byte_cnt = 15;

  /// The encoded bitmap.
  std::vector<byte_t> data_;
  /// The length of the encoded bitmap.
  std::size_t encoded_bitmap_length_ = 0;

  //===--------------------------------------------------------------------===//
  // Helper functions to work with the code words.
  //===--------------------------------------------------------------------===//
  /// The different kind of input bytes to distinguish.
  enum byte_kind {
    ALL_ZERO,
    ALL_ONE,
    ALMOST_ALL_ZERO,
    ALMOST_ALL_ONE,
    MIXED
  };

  /// Determine the kind the byte at index i.
  static inline constexpr byte_kind
  determine_byte_kind(const byte_t* byte_array, const std::size_t byte_array_len,
      const std::size_t i) noexcept {
    assert(i < byte_array_len);
    const byte_t b = byte_array[i];
    if (b == all_zero)
      return ALL_ZERO;
    if (b == all_one)
      return ALL_ONE;
    if (dtl::bits::pop_count(b) == 1) {
      // Could be a dirty 1-bit.
      if (i == 0) return MIXED;
      if (byte_array[i - 1] != all_zero) return MIXED;
      if (((i + 1) < byte_array_len)
        && !(byte_array[i - 1] == all_zero || byte_array[i - 1] == all_one)) return MIXED;
      // Is a dirty 1-bit.
      return ALMOST_ALL_ZERO;
    }
    if (dtl::bits::pop_count(b) == 7) {
      // Could be a dirty 0-bit.
      if (i == 0) return MIXED;
      if (byte_array[i - 1] != all_one) return MIXED;
      if (((i + 1) < byte_array_len)
          && !(byte_array[i - 1] == all_zero || byte_array[i - 1] == all_one)) return MIXED;
      // Is a dirty 0-bit.
      return ALMOST_ALL_ONE;
    }
    return MIXED;
  }
  //===--------------------------------------------------------------------===//

  /// Compress the given input bitmap.
  void
  compress(const byte_t* bitmap_begin, const byte_t* bitmap_end) {
    const byte_t* byte_array = bitmap_begin;
    const std::size_t byte_cnt = bitmap_end - bitmap_begin;

    // The current range of input bytes that are inspected.
    std::size_t b = 0;
    std::size_t e = 0;

    while (e < byte_cnt) {
      // Inspect the input bitmap byte by byte.

      // Counter for the fill bytes.
      std::size_t fill_byte_cnt = 0;
      // The fill value.
      $u1 fill_val = 0;

      // Determine the fill value and length, if there is one.
      if (e < byte_cnt) {
        const auto kind = determine_byte_kind(byte_array, byte_cnt, e);
        if (kind == ALL_ZERO || kind == ALL_ONE) {
          ++e;
          fill_val = kind == ALL_ZERO ? 0 : 1;
          while ((e < byte_cnt)
              && ((e - b) < max_fill_byte_cnt)
              && (determine_byte_kind(byte_array, byte_cnt, e) == kind)) {
            ++e;
          }
          fill_byte_cnt = e - b;
          assert(fill_byte_cnt <= max_fill_byte_cnt);
        }
      }

      // Indicates whether a dirty bit was found in the tail.
      $u1 dirty_tail = false;
      // The position of the dirty bit.
      std::size_t dirty_bit_pos = 0;

      // Check, whether the run has a tail byte with a dirty bit.
      if (e < byte_cnt) {
        const auto kind = determine_byte_kind(byte_array, byte_cnt, e);
        if (kind == ALMOST_ALL_ZERO || kind == ALMOST_ALL_ONE) {
          // Found a literal with a dirty bit.
          dirty_tail = true;
          dirty_bit_pos = dtl::bits::tz_count(
              (kind == ALMOST_ALL_ZERO)
              ? u32(byte_array[e])
              : ~u32(byte_array[e]));
          ++e;
        }
      }

      // Counter for the literal tail bytes.
      std::size_t literal_byte_cnt = 0;
      // The index of the first literal byte in the input bitmap.
      std::size_t literal_bytes_begin = e;
      // The index + 1 of the last literal byte in the input bitmap.
      std::size_t literal_bytes_end = e;

      // Determine the number of tail bytes, if there are any.
      if (!dirty_tail && e < byte_cnt) {
        while ((e < byte_cnt)
            && ((e - b) < max_tail_byte_cnt)
            && (determine_byte_kind(byte_array, byte_cnt, e) == MIXED)) {
          ++e;
        }
        literal_bytes_end = e;
        literal_byte_cnt = literal_bytes_end - literal_bytes_begin;
        assert(literal_byte_cnt <= max_tail_byte_cnt);
      }

      // Write the header.
      byte_t hdr = 0;
      if (fill_byte_cnt < 4) {
        if (!dirty_tail) {
          // BBC case 1
          hdr = (1 << 7)
              | (byte_t(fill_val) << 6)
              | (fill_byte_cnt << 4)
              | literal_byte_cnt;
        }
        else {
          // BBC case 2
          hdr = (1 << 6)
              | (byte_t(fill_val) << 5)
              | (fill_byte_cnt << 3)
              | dirty_bit_pos;
        }
        data_.push_back(hdr);
      }
      else {
        if (!dirty_tail) {
          // BBC case 3
          hdr = (1 << 5)
              | (byte_t(fill_val) << 4)
              | literal_byte_cnt;
        }
        else {
          // BBC case 4
          hdr = (1 << 4)
              | (byte_t(fill_val) << 3)
              | dirty_bit_pos;
        }
        data_.push_back(hdr);
        // Write the variable-byte (VB) counter.
        const auto var_byte_cnt =
            (((sizeof(fill_byte_cnt) * 8) - dtl::bits::lz_count(fill_byte_cnt))
            + 6) / 7;
        for (std::size_t k = 0; k < var_byte_cnt; ++k) {
          const auto o = var_byte_cnt - k - 1;
          byte_t c = (fill_byte_cnt >> (o * 7)) & 0b01111111;
          if (o != 0) c |= 0b10000000;
          data_.push_back(c);
        }
      }

      // Write the literals.
      if (!dirty_tail) {
        for (std::size_t i = literal_bytes_begin; i < literal_bytes_end; ++i) {
          data_.push_back(byte_array[i]);
        }
      }

      b = e;
    }
  }

public:
  bbc() = default;

  explicit bbc(const boost::dynamic_bitset<$u32>& in) {
    // Reinterpret the input bitmap as a byte array.
    const byte_t* bitmap_begin =
        reinterpret_cast<const byte_t*>(in.m_bits.data());
    const byte_t* bitmap_end = bitmap_begin + ((in.size() + 7) / 8);
    // Compress the bitmap.
    compress(bitmap_begin, bitmap_end);
    encoded_bitmap_length_ = in.size();
    // Try to reduce the memory consumption.
    shrink();
  }

  ~bbc() = default;
  bbc(const bbc& other) = default;
  bbc(bbc&& other) noexcept = default;
  bbc& operator=(const bbc& other) = default;
  bbc& operator=(bbc&& other) noexcept = default;

  /// Return the size in bytes.
  std::size_t __forceinline__
  size_in_bytes() const {
    return data_.size() * sizeof(byte_t) /* size of the compressed bitmap */
        + sizeof(encoded_bitmap_length_); /* bit-length of the original bitmap */
  }

  /// Returns the size of the bitmap.
  std::size_t __forceinline__
  size() const {
    return encoded_bitmap_length_;
  }

  /// Conversion to an plain bitmap. // TODO remove
  dtl::plain_bitmap<$u64>
  to_plain_bitmap() const {
    dtl::plain_bitmap<$u64> bitmap(encoded_bitmap_length_);
    byte_t* byte_array = reinterpret_cast<byte_t*>(bitmap.data());
    std::size_t o = 0;
    std::size_t i = 0;
    if (data_.empty()) return bitmap;

    while (i < data_.size()) {
      const auto hdr = data_[i];
      const auto bbc_case = dtl::bits::lz_count(u32(hdr)) - 23;
      switch (bbc_case) {
        case 1: {
          u1 fill_val = (hdr & 0b01000000) != 0;
          const std::size_t fill_byte_cnt = (hdr & 0b00110000) >> 4;
          const std::size_t tail_byte_cnt = hdr & 0b00001111;

          // Write the fill.
          const byte_t fill_byte = fill_val ? all_one : all_zero;
          for (std::size_t k = 0; k < fill_byte_cnt; ++k) {
            byte_array[o + k] = fill_byte;
          }
          o += fill_byte_cnt;

          // Write the literals.
          for (std::size_t k = 0; k < tail_byte_cnt; ++k) {
            byte_array[o + k] = data_[i + 1 + k];
          }
          o += tail_byte_cnt;

          i += 1 + tail_byte_cnt;
          break;
        }

        case 2: {
          u1 fill_val = (hdr & 0b00100000) != 0;
          const std::size_t fill_byte_cnt = (hdr & 0b00011000) >> 3;
          const std::size_t dirty_bit_pos = hdr & 0b00000111;

          // Write the fill.
          const byte_t fill_byte = fill_val ? all_one : all_zero;
          for (std::size_t k = 0; k < fill_byte_cnt; ++k) {
            byte_array[o + k] = fill_byte;
          }
          o += fill_byte_cnt;

          // Write the tail.
          const byte_t dirty_tail_byte = fill_byte ^ (1 << dirty_bit_pos);
          byte_array[o] = dirty_tail_byte;
          o += 1;

          i += 1;
          break;
        }

        case 3: {
          u1 fill_val = (hdr & 0b00010000) != 0;
          // Read the variable-byte (VB) counter.
          std::size_t fill_byte_cnt = 0;
          std::size_t fill_byte_cntr_len = 1;
          for (; fill_byte_cntr_len <= 4; ++fill_byte_cntr_len) {
            const auto x = data_[i + fill_byte_cntr_len];
            fill_byte_cnt <<= 7;
            fill_byte_cnt |= (x & 0b01111111);
            if ((x & 0b10000000) == 0) break;
          }
          const std::size_t tail_byte_cnt = hdr & 0b00001111;

          // Write the fill.
          const byte_t fill_byte = fill_val ? all_one : all_zero;
          for (std::size_t k = 0; k < fill_byte_cnt; ++k) {
            byte_array[o + k] = fill_byte;
          }
          o += fill_byte_cnt;

          // Write the literals.
          for (std::size_t k = 0; k < tail_byte_cnt; ++k) {
            byte_array[o + k] = data_[i + 1 + k + fill_byte_cntr_len];
          }
          o += tail_byte_cnt;

          i += 1 + fill_byte_cntr_len + tail_byte_cnt;
          break;
        }

        case 4: {
          u1 fill_val = (hdr & 0b00001000) != 0;
          // Read the variable-byte (VB) counter.
          std::size_t fill_byte_cnt = 0;
          std::size_t fill_byte_cntr_len = 1;
          for (; fill_byte_cntr_len <= 4; ++fill_byte_cntr_len) {
            const auto x = data_[i + fill_byte_cntr_len];
            fill_byte_cnt <<= 7;
            fill_byte_cnt |= (x & 0b01111111);
            if ((x & 0b10000000) == 0) break;
          }
          const std::size_t dirty_bit_pos = hdr & 0b00000111;

          // Write the fill.
          const byte_t fill_byte = fill_val ? all_one : all_zero;
          for (std::size_t k = 0; k < fill_byte_cnt; ++k) {
            byte_array[o + k] = fill_byte;
          }
          o += fill_byte_cnt;

          // Write the tail.
          const byte_t dirty_tail_byte = fill_byte ^ (1 << dirty_bit_pos);
          byte_array[o] = dirty_tail_byte;
          o += 1;

          i += 1 + fill_byte_cntr_len;
          break;
        }

      }
    }
    return bitmap;
  }

  static std::string
  name() {
    return "bbc";
  }

  /// Returns the value of the bit at the position pos.
  u1 __forceinline__
  test(const std::size_t pos) const {
    const auto dec = to_plain_bitmap(); // FIXME HACK
    return dec.test(pos);
  }

  /// Try to reduce the memory consumption. This function is supposed to be
  /// called after the bitmap has been modified.
  __forceinline__ void
  shrink() {
    data_.shrink_to_fit();
  }

  //===--------------------------------------------------------------------===//
  /// 1-run iterator
  class iter {
    const bbc& outer_;

    //===------------------------------------------------------------------===//
    // Iterator state
    //===------------------------------------------------------------------===//
    using plain_bitmap_t = dtl::plain_bitmap<$u64>;
    std::unique_ptr<plain_bitmap_t> plain_bitmap_; // FIXME HACK
    using nested_iter_t = dtl::plain_bitmap_iter<dtl::plain_bitmap<$u64>>;
    std::unique_ptr<nested_iter_t> plain_bitmap_iter_;
    //===------------------------------------------------------------------===//

  public:
    explicit __forceinline__
    iter(const bbc& outer)
        : outer_(outer) {
      plain_bitmap_ = std::make_unique<plain_bitmap_t>(outer.to_plain_bitmap());
      plain_bitmap_iter_ = std::make_unique<nested_iter_t>(*plain_bitmap_);
    }

    /// Forward the iterator to the next 1-run.
    void __forceinline__
    next() {
      plain_bitmap_iter_->next();
    }

    /// Forward the iterator to the desired position.
    void __forceinline__
    skip_to(const std::size_t to_pos) {
      plain_bitmap_iter_->skip_to(to_pos);
    }

    u1 __forceinline__
    end() const noexcept {
      return plain_bitmap_iter_->end();
    }

    u64 __forceinline__
    pos() const noexcept {
      return plain_bitmap_iter_->pos();
    }

    u64 __forceinline__
    length() const noexcept {
      return plain_bitmap_iter_->length();
    }
  };
  //===--------------------------------------------------------------------===//

  using skip_iter_type = iter;
  using scan_iter_type = iter;

  /// Returns a 1-run iterator.
  skip_iter_type __forceinline__
  it() const {
    return skip_iter_type(*this);
  }

  /// Returns a 1-run iterator.
  scan_iter_type __forceinline__
  scan_it() const {
    return scan_iter_type(*this);
  }

  /// Returns the name of the instance including the most important parameters
  /// in JSON.
  std::string
  info() const {
    return "{\"name\":\"" + name() + "\""
        + ",\"n\":" + std::to_string(encoded_bitmap_length_)
        + ",\"size\":" + std::to_string(size_in_bytes())
        + ",\"word_size\":" + std::to_string(sizeof(byte_t))
        + "}";
  }

  // For debugging purposes.
  void
  print(std::ostream& os) const {
    os << "\nword idx | content" << std::endl;
    for (std::size_t i = 0; i < data_.size(); ++i) {
      os << std::setw(8) << i << " | ";
      auto& w = data_[i];
      os << "'" << std::bitset<8>(w);
      os << std::endl;
    }
  }
};
//===----------------------------------------------------------------------===//
} // namespace dtl


/*ckwg +29
 * Copyright 2018 by Kitware, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *  * Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 *
 *  * Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 *  * Neither name of Kitware, Inc. nor the names of any contributors may be used
 *    to endorse or promote products derived from this software without specific
 *    prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS ``AS IS''
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHORS 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.
 */

/**
 * \file
 * \brief Implementation of refine detections using SVM
 */

#include "refine_detections_with_svm.h"

#include <fstream>
#include <sstream>
#include <iostream>
#include <iomanip>
#include <algorithm>
#include <deque>
#include <vector>

#include <boost/filesystem.hpp>
#include <boost/regex.hpp>

#include <vital/vital_config.h>
#include <vital/logger/logger.h>
#include <vital/exceptions/io.h>

#include <kwiversys/SystemTools.hxx>

#include <svm.h>

using namespace kwiver::vital;

namespace kwiver {
namespace arrows {
namespace svm{

typedef kwiversys::SystemTools ST;

/// Private implementation class
class refine_detections_with_svm::priv
{
 public:

  /// Constructor
  priv()
    : model_dir( "" ),
      override_original( true )
  {
  }

  /// Destructor
  ~priv()
  {
  }

  /// Parameters
  std::string model_dir;

  /// Variables
  bool override_original;

  /// Vector of models
  std::vector< svm_model* > models;
  std::vector< bool > models_first_is_pos;
  std::vector< std::string > model_labels;

  /// Helpers
  void dealloc_models();
  void load_models();

  kwiver::vital::logger_handle_t m_logger;

  std::map<std::string, double> apply_svms( svm_node *x ) const;
};

/// Constructor
refine_detections_with_svm
::refine_detections_with_svm()
    : d_( new priv() )
{
  attach_logger( "arrows.svm.refine_detections_with_svm" );
  d_->m_logger = logger();
}

/// Destructor
refine_detections_with_svm
::~refine_detections_with_svm()
{
  d_->dealloc_models();
}

/// Helper function to deallocate model memory
void
refine_detections_with_svm::priv
::dealloc_models()
{
  for( auto p : models )
  {
    svm_free_and_destroy_model( &p );
  }

  models.clear();
  models_first_is_pos.clear();
  model_labels.clear();
}

/// Helper function to load models from folder
void
refine_detections_with_svm::priv
::load_models()
{
  boost::filesystem::path p( model_dir );
  boost::filesystem::directory_iterator it{ p };

  int *labels = new int[2];

  for (; it != boost::filesystem::directory_iterator {}; ++it)
  {
    std::string file_name_with_path = it->path().string().c_str();
    std::string file_name = (it->path()).filename().string();
    std::string file_name_no_ext = (it->path()).stem().string();

    if( file_name.substr( file_name.find_last_of(".") ) != ".svm" )
    {
      LOG_INFO(m_logger, "Ignoring file without .svm extension: " << file_name);
      continue;
    }

    svm_model *model = svm_load_model(file_name_with_path.c_str());
    int num_classes = svm_get_nr_class(model);
    LOG_ASSERT(m_logger, svm_check_probability_model(model), "Invalid Model");

    // We're expecting a two class problem
    LOG_ASSERT(m_logger, num_classes == 2, "Invalid Model");
    svm_get_labels(model, labels);
    LOG_ASSERT(m_logger, (labels[0] == 1 && labels[1] == -1) ||
        (labels[0] == -1 && labels[1] == 1), "Invalid Model");

    models.push_back( model );
    models_first_is_pos.push_back( labels[0] == 1 );
    model_labels.push_back( file_name_no_ext );
  }

  delete[] labels;
}

/// Get this algorithm's \link vital::config_block configuration block \endlink
vital::config_block_sptr
refine_detections_with_svm
::get_configuration() const
{
  vital::config_block_sptr config = vital::algo::refine_detections::get_configuration();

  config->set_value( "model_dir", d_->model_dir,
                     "The directory where the SVM models are placed." );
  config->set_value( "override_original", d_->override_original,
                     "Replace original scores with new scores." );

  return config;
}

/// Set this algorithm's properties via a config block
void
refine_detections_with_svm
::set_configuration( vital::config_block_sptr in_config )
{
  vital::config_block_sptr config = this->get_configuration();
  config->merge_config( in_config );

  d_->model_dir = config->get_value<std::string>( "model_dir" );
  d_->override_original = config->get_value<bool>( "override_original" );

  d_->dealloc_models();
  d_->load_models();
}

/// Check that the algorithm's currently configuration is valid
bool
refine_detections_with_svm
::check_configuration(vital::config_block_sptr config) const
{
  return true;
}

std::map<std::string, double>
refine_detections_with_svm::priv::apply_svms( svm_node *x ) const
{
  std::map< std::string, double > result;

  for( unsigned i = 0; i < models.size(); ++i )
  {
    double *prob_estimates = new double[2];
    svm_predict_probability( models[i], x, prob_estimates );
    result[ model_labels[i] ] = models_first_is_pos[i] ? prob_estimates[0] : prob_estimates[1];
    delete[] prob_estimates;
  }

  return result;
}

vital::detected_object_set_sptr
refine_detections_with_svm
::refine( vital::image_container_sptr image_data,
          vital::detected_object_set_sptr detections ) const
{

  if( !detections )
  {
    return detections;
  }

  for( auto det : *detections )
  {
    std::vector<double> descriptor_vector = det->descriptor()->as_double();

    size_t descriptor_size = descriptor_vector.size();
    svm_node *svm_nodes = new svm_node[descriptor_size];

    for( size_t i = 0; i < descriptor_size; ++i )
    {
      svm_nodes[i].index = i;
      svm_nodes[i].value = descriptor_vector.at(i);
    }

    typedef std::map<std::string, double> result_map;
    result_map res = d_->apply_svms( svm_nodes );

    // Set output detected object type using map
    detected_object_type_sptr new_type;

    if( d_->override_original || !det->type() )
    {
      new_type = std::make_shared< detected_object_type >();
    }
    else
    {
      new_type = det->type();
    }

    double max_score = 0.0;

    for( result_map::iterator it = res.begin(); it != res.end(); ++it )
    {
      new_type->set_score( it->first, it->second );

      max_score = std::max( max_score, it->second );
    }

    det->set_confidence( max_score );
    det->set_type( new_type );

    delete[] svm_nodes;
  }

  return detections;
}

} // end namespace svm
} // end namespace arrows
} // end namespace kwiver


// __BEGIN_LICENSE__
// Copyright (C) 2006-2011 United States Government as represented by
// the Administrator of the National Aeronautics and Space Administration.
// All Rights Reserved.
// __END_LICENSE__

/// \file dem_profile2.cc
///

// Standard
#include <iostream>

// Vision Workbench
#include <vw/Image.h>
#include <vw/FileIO.h>
#include <vw/Cartography.h>
using namespace vw;

// Boost
#include <boost/program_options.hpp>
namespace po = boost::program_options;

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

  std::string input_file;
  std::string start_ll, end_ll;
  std::string start_px, end_px;
  double idx_column, idx_row;
  double nodata_value;

  po::variables_map vm;

  po::options_description general_options("Options");
  general_options.add_options()
    ("col", po::value<double>(&idx_column), "Process only a single column at this index")
    ("row", po::value<double>(&idx_row), "Process only a single row at this index")
    ("nodata-value", po::value<double>(&nodata_value), "Set the nodata value that is used in the DEM")
    //("ll-start", po::value<std::string>(&start_ll), "Start a profile at this Lon Lat location")
    //("ll-end", po::value<std::string>(&end_ll), "End a profile at this Lon Lat location")
    ("px-start", po::value<std::string>(&start_px)->composing(), "Start a profile at this pixel location")
    ("px-end", po::value<std::string>(&end_px), "End a profile at this pixel location")
    ("help,h", "Display this help message");

  po::options_description hidden_options("");
  hidden_options.add_options()
    ("input-dem", po::value<std::string>(&input_file));

  po::options_description options("Allowed Options");
  options.add(general_options).add(hidden_options);

  po::positional_options_description pos;
  pos.add("input-dem", 1);

  po::store( po::command_line_parser( argc, argv ).options(options).positional(pos).run(), vm );
  po::notify( vm );

  std::ostringstream usage;
  usage << "Usage: " << argv[0] << " <dem-file> [options] \n\n";
  usage << general_options << "\n";

  if ( vm.count("help") || !vm.count("input-dem") ) {
    vw_out() << usage.str() << "\n";
    return 1;
  } else if ( vm.count("ll-start") ^ vm.count("ll-end") ) {
    vw_out() << "Must specify both ends if creating a Lat Lon Profile!\n";
    vw_out() << usage.str() << "\n";
    return 1;
  } else if ( vm.count("px-start") ^ vm.count("px-end") ) {
    vw_out() << "Must specify both ends if creating a Pixel Profile!\n";
    vw_out() << usage.str() << "\n";
    return 1;
  } else if ( vm.count("ll-start") && ( start_ll.size() != 2 ||
                                        end_ll.size() != 2 ) ) {
    vw_out() << "Start and End Lon Lat locations require 2 values, longitude and latitude.\n";
  }

  // Finally starting to perform some work
  cartography::GeoReference georef;
  cartography::read_georeference(georef, input_file);
  DiskImageView<float> disk_dem_file( input_file );
  ImageViewRef<PixelMask<float> > dem;
  if ( vm.count("nodata-value") ) {
    vw_out() << "\t--> Masking nodata value: " << nodata_value << "\n";
    dem = interpolate(create_mask( disk_dem_file, nodata_value ),
                      BicubicInterpolation(),
                      ZeroEdgeExtension());
  } else {
    DiskImageResource *disk_dem_rsrc = DiskImageResource::open(input_file);
    if ( disk_dem_rsrc->has_nodata_read() ) {
      nodata_value = disk_dem_rsrc->nodata_read();
      vw_out() << "\t--> Extracted nodata value from file: " << nodata_value << "\n";
      dem = interpolate(create_mask( disk_dem_file, nodata_value ),
                        BicubicInterpolation(),
                        ZeroEdgeExtension());
    } else {
      dem = interpolate(pixel_cast<PixelMask<float> >(disk_dem_file),
                        BicubicInterpolation(),
                        ZeroEdgeExtension());
    }
  }

  Vector2 start, end; // Pixel Locations (everything boils down to it
                      // at the moment :/. Maybe in the future where
                      // flying cars are possible, when a user
                      // provides LL coordinates .. SLERP is used.
  if ( vm.count("col") ) {
    start = Vector2(idx_column, 0);
    end = Vector2(idx_column, dem.rows()-1);
  } else if ( vm.count("row") ) {
    start = Vector2(0,idx_row);
    end = Vector2(dem.cols()-1,idx_row);
  } else if ( vm.count("ll-start") && vm.count("ll-end") ) {
    // Parsing string into a ll coordinate.
    if ( start_ll.find(',') == std::string::npos ||
         end_ll.find(',') == std::string::npos ) {
      vw_out() << "Longitude and Latitude values must be delimited by a comma.\n";
      vw_out() << "\tEx: --ll-start -109.54,33.483\n\n";
      vw_out() << usage.str() << "\n";
      return 1;
    }

    size_t s_break = start_px.find(',');
    size_t e_break = end_px.find(',');

    Vector2 ll_start, ll_end;

  } else if ( vm.count("px-start") && vm.count("px-end") ) {
    // Parsing string into pixel coordinates.
    if ( start_px.find(',') == std::string::npos ||
         end_px.find(',') == std::string::npos ) {
      vw_out() << "Column and Row pixel values must be delimited by a comma.\n";
      vw_out() << "\tEx: --px-start 65,109\n\n";
      vw_out() << usage.str() << "\n";
      return 1;
    }

    size_t s_break = start_px.find(',');
    size_t e_break = end_px.find(',');

    start.x() = atof(start_px.substr(0,s_break).c_str());
    start.y() = atof(start_px.substr(s_break+1, start_px.size()-s_break-1).c_str());
    end.x() = atof(end_px.substr(0,e_break).c_str());
    end.y() = atof(end_px.substr(e_break+1, end_px.size()-e_break-1).c_str());
  } else {
    // Dial down the middle if the user provides only a DEM.
    start = Vector2( dem.cols()/2, 0);
    end = Vector2( dem.cols()/2, dem.rows()-1);
  }

  // Debug
  vw_out() << "Start: " << start << "\n";
  vw_out() << "End:   " << end << "\n";

  // Working out the output file
  BBox2i bound = bounding_box( dem );
  if ( !bound.contains( start ) ||
       !bound.contains( end ) ) {
    vw_out() << "Given coordinates are not with in the image!\nExiting early .....\n";
    return 1;
  }

  std::ofstream output_file( "profile.csv" );
  output_file << std::setprecision(12) << "lon,lat,pix_x,pix_y,altitude,radius\n";

  Vector2i delta = end - start;
  for ( float r = 0; r < 1.0; r +=1/norm_2(delta) ) {
    Vector2 c_idx = Vector2(0.5,0.5) + start + r*delta;

    if ( !dem(int(c_idx.x()),int(c_idx.y())).valid() )
      continue;

    Vector2 lonlat_loc = georef.pixel_to_lonlat( c_idx );
    double altitude = dem( c_idx.x(), c_idx.y() );
    Vector3 xyz_pos = georef.datum().geodetic_to_cartesian( Vector3( lonlat_loc.x(),
                                                                     lonlat_loc.y(),
                                                                     altitude ));
    double radius = norm_2( xyz_pos );

    output_file << lonlat_loc.x() << "," << lonlat_loc.y() << "," << c_idx.x()
                << "," << c_idx.y() << "," << altitude << "," << radius << "\n";
  }
  output_file.close();

  return 0;
}


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2018-2019, LAAS-CNRS
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_

#include "crocoddyl/core/state-base.hpp"
#include "crocoddyl/multibody/costs/cost-sum.hpp"
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>

namespace crocoddyl {

struct DifferentialActionDataAbstract;  // forward declaration

class DifferentialActionModelAbstract {
 public:
  DifferentialActionModelAbstract(StateAbstract& state, unsigned int const& nu, unsigned int const& nr = 1);
  virtual ~DifferentialActionModelAbstract();

  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                    const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u) = 0;
  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                        const Eigen::Ref<const Eigen::VectorXd>& x, const Eigen::Ref<const Eigen::VectorXd>& u,
                        const bool& recalc = true) = 0;
  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();

  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const Eigen::VectorXd>& x);
  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
                const Eigen::Ref<const Eigen::VectorXd>& x);

  unsigned int const& get_nu() const;
  unsigned int const& get_nr() const;
  StateAbstract& get_state() const;

 protected:
  unsigned int nu_;
  unsigned int nr_;
  StateAbstract& state_;
  Eigen::VectorXd unone_;

#ifdef PYTHON_BINDINGS

 public:
  void calc_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                 const Eigen::VectorXd& u = Eigen::VectorXd()) {
    if (u.size() == 0) {
      calc(data, x);
    } else {
      calc(data, x, u);
    }
  }

  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u, const bool& recalc) {
    calcDiff(data, x, u, recalc);
  }
  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                     const Eigen::VectorXd& u) {
    calcDiff(data, x, u, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x) {
    calcDiff(data, x, unone_, true);
  }
  void calcDiff_wrap(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::VectorXd& x,
                     const bool& recalc) {
    calcDiff(data, x, unone_, recalc);
  }

#endif
};

struct DifferentialActionDataAbstract {
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  template <typename Model>
  explicit DifferentialActionDataAbstract(Model* const model)
      : cost(0.),
        xout(model->get_state().get_nv()),
        r(model->get_nr()),
        qcur(model->get_state().get_nq()),
        vcur(model->get_state().get_nv()),
        Fx(model->get_state().get_nv(), model->get_state().get_ndx()),
        Fu(model->get_state().get_nv(), model->get_nu()),
        Lx(model->get_state().get_ndx()),
        Lu(model->get_nu()),
        Lxx(model->get_state().get_ndx(), model->get_state().get_ndx()),
        Lxu(model->get_state().get_ndx(), model->get_nu()),
        Luu(model->get_nu(), model->get_nu()),
        r_ref(&r(0), model->get_nr()),
        Lx_ref(&Lx(0), model->get_state().get_ndx()),
        Lu_ref(&Lu(0), model->get_nu()),
        Lxx_ref(&Lxx(0), model->get_state().get_ndx(), model->get_state().get_ndx()),
        Lxu_ref(&Lxu(0), model->get_state().get_ndx(), model->get_nu()),
        Luu_ref(&Luu(0), model->get_nu(), model->get_nu()) {
    xout.setZero();
    r.setZero();
    qcur.setZero();
    vcur.setZero();
    Fx.setZero();
    Fu.setZero();
    Lx.setZero();
    Lu.setZero();
    Lxx.setZero();
    Lxu.setZero();
    Luu.setZero();
  }

  void shareCostMemory(const boost::shared_ptr<CostDataSum>& costs) {
    // Share memory with the cost data
    new (&r_ref) Eigen::Map<Eigen::VectorXd>(&costs->r(0), costs->r.size());
    new (&Lx_ref) Eigen::Map<Eigen::VectorXd>(&costs->Lx(0), costs->Lx.size());
    new (&Lu_ref) Eigen::Map<Eigen::VectorXd>(&costs->Lu(0), costs->Lu.size());
    new (&Lxx_ref) Eigen::Map<Eigen::MatrixXd>(&costs->Lxx(0), costs->Lxx.rows(), costs->Lxx.cols());
    new (&Lxu_ref) Eigen::Map<Eigen::MatrixXd>(&costs->Lxu(0), costs->Lxu.rows(), costs->Lxu.cols());
    new (&Luu_ref) Eigen::Map<Eigen::MatrixXd>(&costs->Luu(0), costs->Luu.rows(), costs->Luu.cols());
  }

  const double& get_cost() const { return cost; }
  const Eigen::VectorXd& get_xout() const { return xout; }

  Eigen::VectorXd get_r() const { return r_ref; }
  Eigen::VectorXd get_Lx() const { return Lx_ref; }
  Eigen::VectorXd get_Lu() const { return Lu_ref; }
  Eigen::MatrixXd get_Lxx() const { return Lxx_ref; }
  Eigen::MatrixXd get_Lxu() const { return Lxu_ref; }
  Eigen::MatrixXd get_Luu() const { return Luu_ref; }
  const Eigen::MatrixXd& get_Fx() const { return Fx; }
  const Eigen::MatrixXd& get_Fu() const { return Fu; }

  void set_r(Eigen::VectorXd _r) { r_ref = _r; }
  void set_Lx(Eigen::VectorXd _Lx) { Lx_ref = _Lx; }
  void set_Lu(Eigen::VectorXd _Lu) { Lu_ref = _Lu; }
  void set_Lxx(Eigen::MatrixXd _Lxx) { Lxx_ref = _Lxx; }
  void set_Lxu(Eigen::MatrixXd _Lxu) { Lxu_ref = _Lxu; }
  void set_Luu(Eigen::MatrixXd _Luu) { Luu_ref = _Luu; }

  double cost;
  Eigen::VectorXd xout;
  Eigen::VectorXd r;
  Eigen::VectorXd qcur;
  Eigen::VectorXd vcur;
  Eigen::MatrixXd Fx;
  Eigen::MatrixXd Fu;
  Eigen::VectorXd Lx;
  Eigen::VectorXd Lu;
  Eigen::MatrixXd Lxx;
  Eigen::MatrixXd Lxu;
  Eigen::MatrixXd Luu;

  // Using Map for sharing memory with cost-sum data
  Eigen::Map<Eigen::VectorXd> r_ref;
  Eigen::Map<Eigen::VectorXd> Lx_ref;
  Eigen::Map<Eigen::VectorXd> Lu_ref;
  Eigen::Map<Eigen::MatrixXd> Lxx_ref;
  Eigen::Map<Eigen::MatrixXd> Lxu_ref;
  Eigen::Map<Eigen::MatrixXd> Luu_ref;
};

}  // namespace crocoddyl

#endif  // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_


#include <sstream>

#include <boost/regex.hpp>
#include <boost/lexical_cast.hpp>

#include <aoRecord.h>
#include <aiRecord.h>

#include "flame.h"

namespace {

template<typename REC> // REC is aoRecord or aiRecord
long setting_init_a(REC *prec, DBLINK *plink)
{
    try {
        // "simname elementname param"  implies inst==0
        // "simname elementname[inst] param"
        static boost::regex linkpat("(\\S+) ([^\\s\\[]+)(?:\\[(\\d+)\\])? (\\S+)");

        boost::cmatch M;
        if(!boost::regex_match(plink->value.instio.string, M, linkpat))
            throw std::runtime_error("Bad link string");

        flame::auto_ptr<SimDevSetting> priv(new SimDevSetting);
        priv->prec = (dbCommon*)prec;

        if(!find(SimGlobal.sims, M.str(1), priv->sim))
            throw std::runtime_error("No such simulation instance");

        size_t inst = 0;
        if(M[3].matched)
            inst = boost::lexical_cast<size_t>(M.str(3));

        ElementVoid* elem = priv->sim->machine->find(M.str(2), inst);
        if(!elem)
            throw std::runtime_error("No such element");
        priv->element_index = elem->index;

        priv->param = M.str(4);

        // fetch current value to initialize VAL
        // autosave may overwrite
        double initval = elem->conf().get<double>(priv->param);

        if (prec->aslo) initval *= prec->aslo;
        initval += prec->aoff;

        if(prec->linr && prec->eslo!=0) initval = initval*prec->eslo + prec->eoff;

        prec->val = initval;
        prec->udf = 0;
        // Some minor mischief
        // we don't want the initial UDF alarm, as VAL is indeed defined
        // however, processing all settings immediately (PINI=YES)
        // is redundant, and slows down initialization unnecessarily.
        // so just clear it now.
        prec->sevr = prec->stat = 0;

        prec->dpvt = priv.release();
        return 2;
    }catch(std::exception& e){
        fprintf(stderr, "%s: init error: %s\n", prec->name, e.what());
        return -1;
    }
}

long setting_init_ao(aoRecord *prec)
{
    return setting_init_a(prec, &prec->out);
}

long setting_init_ai(aiRecord *prec)
{
    long ret = setting_init_a(prec, &prec->inp);
    if(ret==2) ret=0;
    return ret;
}

static
long setting_change_ao(aoRecord *prec)
{
    TRY(SimDevSetting) {
        Guard G(priv->sim->lock);

        double value = prec->val;
        if(prec->linr) {
            if(prec->eslo!=0) value = (value - prec->eoff) / prec->eslo;
        }
        value -= prec->aoff;
        if (prec->aslo != 0) value /= prec->aslo;

        if(priv->test_debug(2))
            errlogPrintf("%s: set %u %s.%s = %f\n",
                        prec->name,
                        (unsigned)priv->element_index,
                        priv->sim->machine->at(priv->element_index)->name.c_str(),
                        priv->param.c_str(),
                        value);

        {
            ElementVoid* elem = priv->sim->machine->at(priv->element_index);
            Config conf(elem->conf());
            conf.set<double>(priv->param, value);

            priv->sim->machine->reconfigure(priv->element_index, conf);
        }

        return 0;
    }CATCH_ALARM()
}

static
long setting_readback_ai(aiRecord *prec)
{
    TRY(SimDevSetting) {
        Guard G(priv->sim->lock);

        ElementVoid* elem = priv->sim->machine->at(priv->element_index);

        double curval = elem->conf().get<double>(priv->param);

        if (prec->aslo) curval *= prec->aslo;
        curval += prec->aoff;

        if(prec->linr && prec->eslo!=0) curval = curval*prec->eslo + prec->eoff;

        prec->val = curval;

        return 2;
    }CATCH_ALARM()
}

} // namespace

DSET6(ao, Setting, setting_init_ao, NULL, setting_change_ao);
DSET6(ai, Setting, setting_init_ai, Sim::io_aftersim, setting_readback_ai);