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

#include "coalescent_model.hpp"

#include <memory>
#include <cmath>
#include <complex>
#include <numeric>
#include <stdexcept>

#include <boost/math/special_functions/binomial.hpp>

#include "utils/maths.hpp"
#include "utils/mappable_algorithms.hpp"

namespace octopus {

CoalescentModel::CoalescentModel(Haplotype reference, Parameters params,
                                 std::size_t num_haplotyes_hint, CachingStrategy caching)
: reference_ {std::move(reference)}
, reference_repeats_ {}
, indel_heterozygosity_model_ {make_indel_model(reference_, {params.indel_heterozygosity}, reference_repeats_)}
, params_ {params}
, haplotypes_ {}
, caching_ {caching}
, index_cache_ {}
, index_flag_buffer_ {}
, k_indel_zero_result_cache_ {2 * num_haplotyes_hint, std::vector<boost::optional<LogProbability>> {}}
{
    if (params_.snp_heterozygosity <= 0 || params_.indel_heterozygosity <= 0) {
        throw std::domain_error {"CoalescentModel: snp and indel heterozygosity must be > 0"};
    }
    site_buffer1_.reserve(128);
    site_buffer2_.reserve(128);
    if (caching == CachingStrategy::address) {
        difference_address_cache_.reserve(num_haplotyes_hint);
    } else if (caching_ == CachingStrategy::value) {
        difference_value_cache_.reserve(num_haplotyes_hint);
        difference_value_cache_.emplace(std::piecewise_construct,
                                        std::forward_as_tuple(reference_),
                                        std::forward_as_tuple());
    }
    k_indel_pos_result_cache_.reserve(2 * num_haplotyes_hint);
}

void CoalescentModel::set_reference(Haplotype reference)
{
    reference_ = std::move(reference);
    if (caching_ == CachingStrategy::address) {
        difference_address_cache_.clear();
    } else if (caching_ == CachingStrategy::value) {
        difference_value_cache_.clear();
        difference_value_cache_.emplace(std::piecewise_construct,
                                        std::forward_as_tuple(reference_),
                                        std::forward_as_tuple());
    }
}

void CoalescentModel::prime(MappableBlock<Haplotype> haplotypes)
{
    haplotypes_ = std::move(haplotypes);
    index_cache_.assign(haplotypes_.size(), boost::none);
    index_flag_buffer_.assign(haplotypes_.size(), false);
}

void CoalescentModel::unprime() noexcept
{
    haplotypes_.clear();
    haplotypes_.shrink_to_fit();
    index_cache_.clear();
    index_cache_.shrink_to_fit();
    index_flag_buffer_.clear();
    index_flag_buffer_.shrink_to_fit();
}

bool CoalescentModel::is_primed() const noexcept
{
    return !index_cache_.empty();
}

CoalescentModel::LogProbability CoalescentModel::evaluate(const Haplotype& haplotype) const
{
    return evaluate(count_segregating_sites(haplotype));
}

CoalescentModel::LogProbability CoalescentModel::evaluate(const std::vector<unsigned>& haplotype_indices) const
{
    return evaluate(count_segregating_sites(haplotype_indices));
}

namespace {

auto powm1(const unsigned i) noexcept // std::pow(-1, i)
{
    return (i % 2 == 0) ? 1 : -1;
}

template <typename RealType>
auto coalescent_real_space(const unsigned n, const unsigned k, const RealType theta)
{
    RealType result {0};
    for (unsigned i {2}; i <= n; ++i) {
        result += powm1(i) * maths::binomial_coefficient<RealType>(n - 1, i - 1) * ((i - 1) / (theta + i - 1)) * std::pow(theta / (theta + i - 1), k);
    }
    return std::log(result);
}

template <typename ForwardIt>
auto complex_log_sum_exp(ForwardIt first, ForwardIt last)
{
    using ComplexType = typename std::iterator_traits<ForwardIt>::value_type;
    const auto l = [] (const auto& lhs, const auto& rhs) { return lhs.real() < rhs.real(); };
    const auto max = *std::max_element(first, last, l);
    return max + std::log(std::accumulate(first, last, ComplexType {},
                                          [max] (const auto curr, const auto x) { return curr + std::exp(x - max); }));
}

template <typename Container>
auto complex_log_sum_exp(const Container& logs)
{
    return complex_log_sum_exp(std::cbegin(logs), std::cend(logs));
}

template <typename RealType>
auto coalescent_log_space(const unsigned n, const unsigned k, const RealType theta)
{
    std::vector<std::complex<RealType>> tmp(n - 1, std::log(std::complex<RealType> {-1}));
    for (unsigned i {2}; i <= n; ++i) {
        auto& cur = tmp[i - 2];
        cur *= i;
        cur += maths::log_binomial_coefficient<RealType>(n - 1, i - 1);
        cur += std::log((i - 1) / (theta + i - 1));
        cur += k * std::log(theta / (theta + i - 1));
    }
    return complex_log_sum_exp(tmp).real();
}

template <typename RealType>
auto coalescent(const unsigned n, const unsigned k, const RealType theta)
{
    if (n < 30 && k <= 80) {
        auto result = coalescent_real_space(n, k, theta);
        if (std::isnan(result)) {
            result = coalescent_log_space(n, k, theta);
        }
        return result;
    } else {
        return coalescent_log_space(n, k, theta);
    }
}

template <typename RealType>
auto coalescent(const unsigned n, const unsigned k_snp, const unsigned k_indel,
                const RealType theta_snp, const RealType theta_indel)
{
    const auto theta = theta_snp + theta_indel;
    const auto k_tot = k_snp + k_indel;
    auto result = coalescent(n, k_tot, theta);
    result += k_snp * std::log(theta_snp / theta);
    result += k_indel * std::log(theta_indel / theta);
    result += maths::log_binomial_coefficient<RealType>(k_tot, k_snp);
    return result;
}

template <typename RealType, std::size_t N>
auto coalescent(const unsigned n,
                const std::array<unsigned, N>& k,
                const std::array<RealType, N>& theta)
{
    const auto theta_tot = std::accumulate(std::cbegin(theta), std::cend(theta), RealType {0});
    const auto k_tot = std::accumulate(std::cbegin(k), std::cend(k), 0u);
    auto result = coalescent(n, k_tot, theta_tot);
    for (std::size_t i {0}; i < N; ++i) {
        result += k[i] * std::log(theta[i] / theta_tot);
    }
    result += maths::log_multinomial_coefficient<RealType>(k);
    return result;
}

} // namespace

CoalescentModel::LogProbability CoalescentModel::evaluate(const SegregatingSiteCounts& counts) const
{
    if (counts.repeat_indels + counts.complex_indels == 0) {
        return evaluate_no_indels(counts.snps, counts.haplotypes);
    } else {
        const auto indel_heterozygosities = calculate_buffered_indel_heterozygosities();
        const auto repeat_indel_heterozygosity = maths::round_sf(indel_heterozygosities.second, 6);
        const auto complex_indel_heterozygosity = maths::round_sf(indel_heterozygosities.first, 6);
        SegregatingSiteCountsWithIndelHeterozygosities sites {counts, repeat_indel_heterozygosity, complex_indel_heterozygosity};
        auto itr = k_indel_pos_result_cache_.find(sites);
        if (itr != std::cend(k_indel_pos_result_cache_)) {
            return itr->second;
        }
        const std::array<unsigned, 3> site_counts {counts.snps, counts.repeat_indels, counts.complex_indels};
        const std::array<double, 3> site_heterozygosities {params_.snp_heterozygosity, repeat_indel_heterozygosity, complex_indel_heterozygosity};
        const auto result = coalescent(counts.haplotypes, site_counts, site_heterozygosities);
        k_indel_pos_result_cache_.emplace(sites, result);
        return result;
    }
}

CoalescentModel::LogProbability CoalescentModel::evaluate_no_indels(const unsigned k_snp, const unsigned n) const
{
    if (k_indel_zero_result_cache_.size() > n) {
        if (k_indel_zero_result_cache_[n].size() > k_snp) {
            auto& result = k_indel_zero_result_cache_[n][k_snp];
            if (!result) {
                result = coalescent(n, k_snp, 0, params_.snp_heterozygosity, params_.indel_heterozygosity);
            }
            return *result;
        } else {
            k_indel_zero_result_cache_[n].resize(k_snp + 1, boost::none);
        }
    } else {
        k_indel_zero_result_cache_.resize(n + 1);
        k_indel_zero_result_cache_[n].assign(k_snp + 1, boost::none);
    }
    const auto result = coalescent(n, k_snp, 0, params_.snp_heterozygosity, params_.indel_heterozygosity);
    k_indel_zero_result_cache_[n][k_snp] = result;
    return result;
}

void CoalescentModel::fill_site_buffer(const Haplotype& haplotype) const
{
    assert(site_buffer2_.empty());
    site_buffer1_.clear();
    if (caching_ == CachingStrategy::address) {
        fill_site_buffer_from_address_cache(haplotype);
    } else {
        fill_site_buffer_from_value_cache(haplotype);
    }
    site_buffer1_ = std::move(site_buffer2_);
    site_buffer2_.clear();
}

void CoalescentModel::fill_site_buffer_uncached(const Haplotype& haplotype) const
{
    // Although we won't retrieve from the cache, we need to make sure all the variants
    // stay in existence as we populate the buffers by reference.
    auto itr = difference_value_cache_.find(reference_);
    if (itr == std::cend(difference_value_cache_)) {
        itr = difference_value_cache_.emplace(reference_, haplotype.difference(reference_)).first;
    } else {
        itr->second = haplotype.difference(reference_);
    }
    std::set_union(std::begin(site_buffer1_), std::end(site_buffer1_),
                   std::cbegin(itr->second), std::cend(itr->second),
                   std::back_inserter(site_buffer2_));
}

void CoalescentModel::fill_site_buffer_from_value_cache(const Haplotype& haplotype) const
{
    auto itr = difference_value_cache_.find(haplotype);
    if (itr == std::cend(difference_value_cache_)) {
        itr = difference_value_cache_.emplace(haplotype, haplotype.difference(reference_)).first;
    }
    std::set_union(std::begin(site_buffer1_), std::end(site_buffer1_),
                   std::cbegin(itr->second), std::cend(itr->second),
                   std::back_inserter(site_buffer2_));
}

void CoalescentModel::fill_site_buffer_from_address_cache(const Haplotype& haplotype) const
{
    auto itr = difference_address_cache_.find(std::addressof(haplotype));
    if (itr == std::cend(difference_address_cache_)) {
        itr = difference_address_cache_.emplace(std::addressof(haplotype), haplotype.difference(reference_)).first;
    }
    std::set_union(std::begin(site_buffer1_), std::end(site_buffer1_),
                   std::cbegin(itr->second), std::cend(itr->second),
                   std::back_inserter(site_buffer2_));
}

CoalescentModel::SegregatingSiteCounts CoalescentModel::count_segregating_sites(const Haplotype& haplotype) const
{
    fill_site_buffer(haplotype);
    return count_segregating_sites_in_buffer(1);
}

CoalescentModel::SegregatingSiteCounts CoalescentModel::count_segregating_sites_in_buffer(const unsigned num_haplotypes) const
{
    unsigned repeat_indels {0}, complex_indels {0};
    for (const auto& site : site_buffer1_) {
        if (is_indel(site)) {
            if (has_overlapped(reference_repeats_, site.get())) {
                ++repeat_indels;
            } else {
                ++complex_indels;
            }
        }
    }
    const auto num_indels = repeat_indels + complex_indels;
    const auto num_snps = static_cast<unsigned>(site_buffer1_.size()) - num_indels;
    return {num_haplotypes + 1, num_snps, repeat_indels, complex_indels};
}

std::pair<double, double> CoalescentModel::calculate_buffered_indel_heterozygosities() const
{
    boost::optional<double> min_heterozygosity {}, max_heterozygosity {};
    for (const auto& site : site_buffer1_) {
        if (is_indel(site)) {
            auto site_heterozygosity = calculate_heterozygosity(site);
            if (min_heterozygosity) {
                min_heterozygosity = std::min(*min_heterozygosity, site_heterozygosity);
            } else {
                min_heterozygosity = site_heterozygosity;
            }
            if (max_heterozygosity) {
                max_heterozygosity = std::max(*max_heterozygosity, site_heterozygosity);
            } else {
                max_heterozygosity = site_heterozygosity;
            }
        }
    }
    if (!min_heterozygosity) min_heterozygosity = params_.indel_heterozygosity;
    if (!max_heterozygosity) max_heterozygosity = params_.indel_heterozygosity;
    return std::make_pair(*min_heterozygosity, *max_heterozygosity);
}

double CoalescentModel::calculate_heterozygosity(const Variant& indel) const
{
    assert(is_indel(indel));
    const auto offset = static_cast<std::size_t>(begin_distance(reference_, indel));
    return calculate_indel_probability(indel_heterozygosity_model_, offset, indel_size(indel));
}

CoalescentProbabilityGreater::CoalescentProbabilityGreater(CoalescentModel model)
: model_ {std::move(model)}
, buffer_ {}
, cache_ {}
{
    buffer_.reserve(1);
    cache_.reserve(100);
}

bool CoalescentProbabilityGreater::operator()(const Haplotype& lhs, const Haplotype& rhs) const
{
    if (have_same_alleles(lhs, rhs)) return true;
    auto cache_itr = cache_.find(lhs);
    if (cache_itr == std::cend(cache_)) {
        buffer_.assign({lhs});
        cache_itr = cache_.emplace(lhs, model_.evaluate(buffer_)).first;
    }
    const auto lhs_probability = cache_itr->second;
    cache_itr = cache_.find(rhs);
    if (cache_itr == std::cend(cache_)) {
        buffer_.assign({rhs});
        cache_itr = cache_.emplace(rhs, model_.evaluate(buffer_)).first;
    }
    const auto rhs_probability = cache_itr->second;
    return lhs_probability > rhs_probability;
}

bool operator==(const CoalescentModel::SegregatingSiteCounts& lhs, const CoalescentModel::SegregatingSiteCounts& rhs) noexcept
{
    return lhs.haplotypes == rhs.haplotypes && lhs.snps == rhs.snps && lhs.repeat_indels == rhs.repeat_indels && lhs.complex_indels == rhs.complex_indels;
}

std::size_t CoalescentModel::SegregatingSiteCountsHash::operator()(const SegregatingSiteCounts& counts) const noexcept
{
    return boost::hash_value(std::make_tuple(counts.haplotypes, counts.snps, counts.repeat_indels, counts.complex_indels));
}

std::size_t CoalescentModel::SegregatingSiteCountsWithIndelHeterozygositiesHash::operator()(const SegregatingSiteCountsWithIndelHeterozygosities& sites) const noexcept
{
    using boost::hash_combine;
    std::size_t result {};
    hash_combine(result, SegregatingSiteCountsHash{}(sites.counts));
    hash_combine(result, std::hash<decltype(sites.repeat_heterozygosity)>()(sites.repeat_heterozygosity));
    hash_combine(result, std::hash<decltype(sites.complex_heterozygosity)>()(sites.complex_heterozygosity));
    return result;
}

bool CoalescentModel::SegregatingSiteCountsWithIndelHeterozygositiesEqual::operator()(const SegregatingSiteCountsWithIndelHeterozygosities& lhs, const SegregatingSiteCountsWithIndelHeterozygosities& rhs) const noexcept
{
    return lhs.counts == rhs.counts && lhs.repeat_heterozygosity == rhs.repeat_heterozygosity && lhs.complex_heterozygosity == rhs.complex_heterozygosity;
}

} // namespace octopus


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/Interpret.h"

#include "Utils/Geometry/AtomCollection.h"
#include "Utils/Bonds/BondOrderCollection.h"

#include "Molassembler/BondOrders.h"
#include "Molassembler/Detail/Cartesian.h"
#include "Molassembler/Graph.h"
#include "Molassembler/Graph/GraphAlgorithms.h"
#include "Molassembler/Graph/PrivateGraph.h"
#include "Molassembler/Molecule.h"
#include "Molassembler/Shapes/ContinuousMeasures.h"
#include "Molassembler/Temple/Adaptors/AllPairs.h"
#include "Molassembler/Temple/Functional.h"
#include "Molassembler/Temple/Optionals.h"

#include <Eigen/Geometry>

namespace Scine {
namespace Molassembler {
namespace Interpret {
namespace {

Utils::PositionCollection paste(const std::vector<Utils::Position>& positions) {
  const unsigned N = positions.size();
  auto matrix = Utils::PositionCollection(N, 3);
  for(unsigned i = 0; i < N; ++i) {
    matrix.row(i) = positions.at(i);
  }
  return matrix;
}

double vectorAngle(const Eigen::Vector3d& a, const Eigen::Vector3d& b) {
  return std::acos(
    a.dot(b) / (
      a.norm() * b.norm()
    )
  );
}

struct HapticPlaneGeometry {
  double angle;
  double rmsd;
};

HapticPlaneGeometry hapticPlaneGeometry(
  const std::vector<Utils::Position>& positions,
  const AtomIndex v,
  const std::vector<AtomIndex>& site
) {
  const unsigned siteSize = site.size();

  assert(siteSize >= 1 && "Passed hapticPlaneGeometry a non-haptic site!");

  Eigen::Vector3d siteCentroid = Eigen::Vector3d::Zero();
  for(AtomIndex siteAtom : site) {
    siteCentroid += positions.at(siteAtom);
  }
  siteCentroid /= siteSize;

  if(siteSize == 2) {
    const double frontAngle = Cartesian::angle(
      positions.at(v),
      siteCentroid,
      positions.at(site.front())
    );
    const double backAngle = Cartesian::angle(
      positions.at(v),
      siteCentroid,
      positions.at(site.back())
    );

    return HapticPlaneGeometry {
      M_PI / 2.0 - std::min(frontAngle, backAngle),
      0.0
    };
  }

  const Eigen::Vector3d centroidAxis = siteCentroid - positions.at(v).transpose();

  Utils::PositionCollection hapticAtoms(siteSize, 3);
  for(unsigned i = 0; i < siteSize; ++i) {
    hapticAtoms.row(i) = positions.at(site.at(i));
  }
  const auto plane = Cartesian::planeOfBestFit(hapticAtoms);
  const double firstAngle = vectorAngle(plane.normal(), centroidAxis);
  const double secondAngle = vectorAngle(plane.normal(), -centroidAxis);
  return HapticPlaneGeometry {
    std::min(firstAngle, secondAngle),
    Cartesian::planeRmsd(plane, hapticAtoms, Temple::iota<AtomIndex>(siteSize))
  };
}

boost::optional<double> minimumClassificationProbability(
  const PrivateGraph& graph,
  const std::vector<Utils::Position>& positions,
  const AtomIndex v
) {
  auto sites = GraphAlgorithms::sites(graph, v);
  if(sites.size() <= 1) {
    return boost::none;
  }

  const unsigned S = sites.size();
  Eigen::Matrix<double, 3, Eigen::Dynamic> sitePositions(3, S + 1);
  for(unsigned i = 0; i < S; ++i) {
    Eigen::Vector3d averagePosition = Eigen::Vector3d::Zero();
    for(unsigned j : sites[i]) {
      averagePosition += positions.at(j);
    }
    sitePositions.col(i) = averagePosition / sites[i].size();
  }
  sitePositions.col(S) = positions.at(v);

  auto normalizedPositions = Shapes::Continuous::normalize(sitePositions);

  // Classify all suitable shapes
  std::vector<Shapes::Shape> viableShapes;
  for(const Shapes::Shape shape : Shapes::allShapes) {
    if(Shapes::size(shape) == S) {
      viableShapes.push_back(shape);
    }
  }

  const auto classifications = Temple::map(viableShapes,
    [&](const Shapes::Shape shape) -> boost::optional<double> {
      const double measure = Shapes::Continuous::shapeCentroidLast(normalizedPositions, shape).measure;
      return Shapes::Continuous::probabilityRandomCloud(measure, shape);
    }
  );

  if(Temple::all_of(classifications)) {
    const double minimumProbability = std::min_element(
      std::begin(classifications),
      std::end(classifications),
      [](const auto& a, const auto& b) -> bool {
        return a.value() < b.value();
      }
    )->value();

    return minimumProbability;
  }

  return boost::none;
}

struct BestRemovals {
  std::vector<AtomIndex> removals;
  double certainty;
};

boost::optional<BestRemovals> bestRemovalFromHapticSite(
  const PrivateGraph& graph,
  const std::vector<Utils::Position>& positions,
  const AtomIndex v,
  const std::vector<std::vector<AtomIndex>>& sites,
  const unsigned hapticSiteIndex
) {
  const unsigned S = sites.size();
  const std::vector<AtomIndex>& hapticSite = sites.at(hapticSiteIndex);
  const unsigned hapticSiteSize = hapticSite.size();
  return Temple::accumulate(
    hapticSite,
    boost::optional<BestRemovals>(boost::none),
    [&](const auto& carry, const AtomIndex siteVertexToRemove) -> boost::optional<BestRemovals> {
      auto graphCopy = graph;
      graphCopy.removeEdge(graphCopy.edge(v, siteVertexToRemove));
      GraphAlgorithms::updateEtaBonds(graphCopy);
      const auto newSites = GraphAlgorithms::sites(graphCopy, v);

      /* Possible effects of bond removal
       * - Separating a haptic ligand into two single-atom ligands,
       *   changing shapes
       *   - recognizable by size change
       *   - accept if new certainty is low or significantly
       *     lower than old (factor 0.5)
       * - Improves the haptic plane angle by removing a bad bond
       *   - recognized by matching sites and comparing angles
       *   - accept if angle halves
       *
       * How to compare cases?
       */
      if(newSites.size() > S) {
        const auto priorCertainty = minimumClassificationProbability(graph, positions, v);
        const auto posteriorCertainty = minimumClassificationProbability(graphCopy, positions, v);

        if(priorCertainty && posteriorCertainty) {
          if(
            posteriorCertainty.value() <= 0.01
            || posteriorCertainty.value() <= 0.5 * priorCertainty.value()
          ) {
            return BestRemovals {{siteVertexToRemove}, 1 - *posteriorCertainty};
          }
        }
      } else {
        const auto siteFindIter = Temple::find_if(
          newSites,
          [&](const auto& newSite) -> bool {
            if(newSite.size() != hapticSiteSize - 1) {
              return false;
            }

            // Match all vertices except the one to remove
            for(const AtomIndex oldSiteAtomIndex : hapticSite) {
              if(oldSiteAtomIndex == siteVertexToRemove) {
                continue;
              }

              if(Temple::find(newSite, oldSiteAtomIndex) == std::end(newSite)) {
                return false;
              }
            }

            return true;
          }
        );

        if(siteFindIter != std::end(newSites)) {
          const double newHapticAngle = hapticPlaneGeometry(positions, v, *siteFindIter).angle;
          const double newHapticAngleCertainty = 1 - newHapticAngle * 2 / M_PI;
          const double carryCertainty = Temple::Optionals::map(carry,
            [](const auto& removal) { return removal.certainty; }
          ).value_or(0.0);
          if(newHapticAngleCertainty > carryCertainty) {
            return BestRemovals {{siteVertexToRemove}, newHapticAngleCertainty};
          }
        }
      }

      return carry;
    }
  );
}

} // namespace

std::vector<std::vector<unsigned>> ComponentMap::invert() const {
  const unsigned nComponents = *std::max_element(
    std::begin(map),
    std::end(map)
  ) + 1;

  std::vector<
    std::vector<unsigned>
  > inverseMaps (nComponents);

  const unsigned N = map.size();
  for(unsigned i = 0; i < N; ++i) {
    inverseMaps.at(map.at(i)).push_back(i);
  }

  return inverseMaps;
}

std::vector<Utils::AtomCollection> ComponentMap::apply(
  const Utils::AtomCollection& atomCollection
) const {
  const unsigned nComponents = *std::max_element(
    std::begin(map),
    std::end(map)
  ) + 1;
  std::vector<unsigned> componentSizes(nComponents, 0);
  for(unsigned i : map) {
    componentSizes.at(i) += 1;
  }

  /* Allocate the collections */
  std::vector<Utils::AtomCollection> collections = Temple::map(
    componentSizes,
    [](const unsigned size) { return Utils::AtomCollection(size); }
  );
  std::vector<unsigned> collectionSizeCount(nComponents, 0);

  for(unsigned i = 0; i < map.size(); ++i) {
    unsigned moleculeIndex = map.at(i);
    Utils::AtomCollection& collection = collections.at(moleculeIndex);
    unsigned& collectionSize = collectionSizeCount.at(moleculeIndex);
    collection.setElement(
      collectionSize,
      atomCollection.getElement(i)
    );
    collection.setPosition(
      collectionSize,
      atomCollection.getPosition(i)
    );
    ++collectionSize;
  }

  return collections;
}

ComponentMap::ComponentIndexPair ComponentMap::apply(const unsigned index) const {
  ComponentIndexPair pair;
  pair.component = map.at(index);
  pair.atomIndex = std::count(
    std::begin(map),
    std::begin(map) + index,
    pair.component
  );
  return pair;
}

unsigned ComponentMap::invert(const ComponentIndexPair& pair) const {
  unsigned count = 0;
  const unsigned N = map.size();
  for(unsigned i = 0; i < N; ++i) {
    if(map[i] == pair.component) {
      ++count;
    }

    if(count == pair.atomIndex + 1) {
      return i;
    }
  }

  throw std::out_of_range("No match found in component map!");
}

struct MoleculeParts {
  PrivateGraph graph;
  std::vector<Utils::Position> angstromPositions;
  boost::optional<
    std::vector<BondIndex>
  > bondStereopermutatorCandidatesOptional;
};

struct Parts {
  std::vector<MoleculeParts> precursors;
  ComponentMap componentMap;
  unsigned nZeroLengthPositions = 0;
};

// Yields a graph structure without element type annotations
PrivateGraph discretize(
  const Utils::BondOrderCollection& bondOrders,
  const BondDiscretizationOption discretization
) {
  const PrivateGraph::Vertex N = bondOrders.getSystemSize();
  PrivateGraph graph {N};

  if(discretization == BondDiscretizationOption::Binary) {
    for(unsigned i = 0; i < N; ++i) {
      for(unsigned j = i + 1; j < N; ++j) {
        double bondOrder = bondOrders.getOrder(i, j);

        if(bondOrder > 0.5) {
          graph.addEdge(i, j, BondType::Single);
        }
      }
    }
  } else if(discretization == BondDiscretizationOption::RoundToNearest) {
    for(unsigned i = 0; i < N; ++i) {
      for(unsigned j = i + 1; j < N; ++j) {
        double bondOrder = bondOrders.getOrder(i, j);

        if(bondOrder > 0.5) {
          auto bond = static_cast<BondType>(
            std::round(bondOrder) - 1
          );

          if(bondOrder > 6.5) {
            bond = BondType::Sextuple;
          }

          graph.addEdge(i, j, bond);
        }
      }
    }
  }

  return graph;
}

Parts construeParts(
  const Utils::ElementTypeCollection& elements,
  const AngstromPositions& angstromWrapper,
  const Utils::BondOrderCollection& bondOrders,
  const BondDiscretizationOption discretization,
  const boost::optional<double>& stereopermutatorThreshold
) {
  const unsigned N = elements.size();

  // Check preconditions
  if(angstromWrapper.positions.rows() != N) {
    throw std::invalid_argument(
      "Number of positions in angstrom wrapper do not match number of elements"
    );
  }

  if(bondOrders.getSystemSize<unsigned>() != N) {
    throw std::invalid_argument(
      "Bond order argument system size does not match number of elements"
    );
  }

  PrivateGraph atomCollectionGraph = discretize(bondOrders, discretization);

  Parts parts;
  const unsigned numComponents = atomCollectionGraph.connectedComponents(parts.componentMap.map);
  parts.precursors.resize(numComponents);

  if(stereopermutatorThreshold) {
    for(auto& precursor : parts.precursors) {
      // Empty-vector-initialize the candidate optionals
      precursor.bondStereopermutatorCandidatesOptional = std::vector<BondIndex> {};
    }
  }

  // Map from original index to component index
  std::vector<PrivateGraph::Vertex> indexInComponentMap (N);

  /* Maybe
   * - filtered_graph using predicate of componentMap number
   * - copy_graph to new Graph keeping element types and bond orders
   *
   * - alternately, must keep a map of atomcollection index to precursor index
   *   and new precursor atom index in order to transfer edges too
   */

  for(unsigned i = 0; i < N; ++i) {
    auto& precursor = parts.precursors.at(
      parts.componentMap.apply(i).component
    );

    // Add a new vertex with element information
    PrivateGraph::Vertex newIndex = precursor.graph.addVertex(elements.at(i));

    // Save new index in precursor graph
    indexInComponentMap.at(i) = newIndex;

    if(angstromWrapper.positions.row(i).norm() <= 1e-14) {
      parts.nZeroLengthPositions += 1;
    }

    // Copy over position information
    precursor.angstromPositions.emplace_back(
      angstromWrapper.positions.row(i)
    );
  }

  // Copy over edges and bond orders
  for(const PrivateGraph::Edge& edge : atomCollectionGraph.edges()) {
    const PrivateGraph::Vertex source = atomCollectionGraph.source(edge);
    const PrivateGraph::Vertex target = atomCollectionGraph.target(edge);

    // Both source and target are part of the same component (since they are bonded)
    auto& precursor = parts.precursors.at(
      parts.componentMap.apply(source).component
    );

    // Copy over the edge
    precursor.graph.addEdge(
      indexInComponentMap.at(source),
      indexInComponentMap.at(target),
      atomCollectionGraph.bondType(edge)
    );

    // If the edge's bond order exceeds the threshold optional
    if(
      stereopermutatorThreshold
      && bondOrders.getOrder(source, target) >= *stereopermutatorThreshold
    ) {
      precursor.bondStereopermutatorCandidatesOptional->emplace_back(
        indexInComponentMap.at(source),
        indexInComponentMap.at(target)
      );
    }
  }

  return parts;
}

MoleculesResult molecules(
  const Utils::ElementTypeCollection& elements,
  const AngstromPositions& angstromWrapper,
  const Utils::BondOrderCollection& bondOrders,
  const BondDiscretizationOption discretization,
  const boost::optional<double>& stereopermutatorThreshold
) {
  Parts parts = construeParts(
    elements,
    angstromWrapper,
    bondOrders,
    discretization,
    stereopermutatorThreshold
  );

  // Collect results
  MoleculesResult result;

  /* Transform precursors into Molecules. Positions may only be used if there
   * is at most one position very close to (0, 0, 0). Otherwise, we assume that
   * the given positions are faulty or no positional information is present,
   * and only the graph is used to create the Molecules.
   */
  if(parts.nZeroLengthPositions < 2) {
    result.molecules.reserve(parts.precursors.size());
    for(auto& precursor : parts.precursors) {
      result.molecules.emplace_back(
        Graph {std::move(precursor.graph)},
        AngstromPositions(paste(precursor.angstromPositions), LengthUnit::Angstrom),
        precursor.bondStereopermutatorCandidatesOptional
      );
    }
  } else {
    result.molecules.reserve(parts.precursors.size());
    for(auto& precursor : parts.precursors) {
      result.molecules.emplace_back(
        Graph {std::move(precursor.graph)}
      );
    }
  }

  result.componentMap = std::move(parts.componentMap);

  return result;
}

MoleculesResult molecules(
  const Utils::ElementTypeCollection& elements,
  const AngstromPositions& angstromWrapper,
  const BondDiscretizationOption discretization,
  const boost::optional<double>& stereopermutatorThreshold
) {
  return molecules(
    elements,
    angstromWrapper,
    uffBondOrders(elements, angstromWrapper),
    discretization,
    stereopermutatorThreshold
  );
}

MoleculesResult molecules(
  const Utils::AtomCollection& atomCollection,
  const Utils::BondOrderCollection& bondOrders,
  const BondDiscretizationOption discretization,
  const boost::optional<double>& stereopermutatorThreshold
) {
  return molecules(
    atomCollection.getElements(),
    AngstromPositions {atomCollection.getPositions(), LengthUnit::Bohr},
    bondOrders,
    discretization,
    stereopermutatorThreshold
  );
}

MoleculesResult molecules(
  const Utils::AtomCollection& atomCollection,
  const BondDiscretizationOption discretization,
  const boost::optional<double>& stereopermutatorThreshold
) {
  AngstromPositions angstromWrapper {atomCollection.getPositions(), LengthUnit::Bohr};

  return molecules(
    atomCollection.getElements(),
    angstromWrapper,
    uffBondOrders(atomCollection.getElements(), angstromWrapper),
    discretization,
    stereopermutatorThreshold
  );
}

GraphsResult graphs(
  const Utils::ElementTypeCollection& elements,
  const AngstromPositions& angstromWrapper,
  const Utils::BondOrderCollection& bondOrders,
  BondDiscretizationOption discretization
) {
  Parts parts = construeParts(
    elements,
    angstromWrapper,
    bondOrders,
    discretization,
    boost::none
  );

  GraphsResult result;
  result.graphs.reserve(parts.precursors.size());
  for(auto& precursor : parts.precursors) {
    result.graphs.emplace_back(std::move(precursor.graph));
  }
  result.componentMap = std::move(parts.componentMap);
  return result;
}

GraphsResult graphs(
  const Utils::AtomCollection& atomCollection,
  const Utils::BondOrderCollection& bondOrders,
  BondDiscretizationOption discretization
) {
  return graphs(
    atomCollection.getElements(),
    AngstromPositions {atomCollection.getPositions(), LengthUnit::Bohr},
    bondOrders,
    discretization
  );
}

std::vector<FalsePositive> uncertainBonds(
  const Utils::AtomCollection& atomCollection,
  const Utils::BondOrderCollection& bondOrders
) {
  std::vector<FalsePositive> falsePositives;

  Parts parts = construeParts(
    atomCollection.getElements(),
    AngstromPositions {atomCollection.getPositions(), LengthUnit::Bohr},
    bondOrders,
    BondDiscretizationOption::Binary,
    boost::none
  );

  for(unsigned component = 0; component < parts.precursors.size(); ++component) {
    MoleculeParts& part = parts.precursors[component];
    GraphAlgorithms::updateEtaBonds(part.graph);

    std::vector<std::pair<PrivateGraph::Vertex, double>> sketchyClassifications;

    for(const PrivateGraph::Vertex v : part.graph.vertices()) {
      /* Detect high uncertainty shape classifications. Adjacent pairs with high
       * uncertainties are candidates for false positives.
       */
      auto classificationUncertainty = minimumClassificationProbability(part.graph, part.angstromPositions, v);
      if(classificationUncertainty && *classificationUncertainty >= 0.5) {
        sketchyClassifications.emplace_back(v, *classificationUncertainty);
      }
    }

    // NOTE: Cannot remove bonds with overlapping constituent atoms!
    for(const auto& sketchyPair : Temple::Adaptors::allPairs(sketchyClassifications)) {
      const unsigned i = sketchyPair.first.first;
      const unsigned j = sketchyPair.second.first;
      const double i_random = sketchyPair.first.second;
      const double j_random = sketchyPair.second.second;

      if(part.graph.edgeOption(i, j)) {
        falsePositives.push_back(
          FalsePositive {
            parts.componentMap.invert(ComponentMap::ComponentIndexPair {component, i}),
            parts.componentMap.invert(ComponentMap::ComponentIndexPair {component, j}),
            i_random * j_random
          }
        );
      }
    }
  }

  return falsePositives;
}

std::vector<FalsePositive> badHapticLigandBonds(
  const Utils::AtomCollection& atomCollection,
  const Utils::BondOrderCollection& bondOrders
) {
  std::vector<FalsePositive> falsePositives;

  // Avoid duplicate bonds in list with reverse i-j
  auto addFalsePositive = [&](unsigned i, unsigned j, double p) {
    if(i > j) {
      std::swap(i, j);
    }
    auto findIter = std::find_if(
      std::begin(falsePositives),
      std::end(falsePositives),
      [=](const FalsePositive& fp) -> bool {
        return fp.i == i && fp.j == j;
      }
    );
    if(findIter == std::end(falsePositives)) {
      falsePositives.push_back(FalsePositive {i, j, p});
    }
  };

  Parts parts = construeParts(
    atomCollection.getElements(),
    AngstromPositions {atomCollection.getPositions(), LengthUnit::Bohr},
    bondOrders,
    BondDiscretizationOption::Binary,
    boost::none
  );

  for(unsigned component = 0; component < parts.precursors.size(); ++component) {
    MoleculeParts& part = parts.precursors[component];
    GraphAlgorithms::updateEtaBonds(part.graph);

    for(const PrivateGraph::Vertex v : part.graph.vertices()) {
      /* Detect haptic shape planes with large angles to the axis defined by the
       * site position and the central atom or with high rms deviations on
       * their plane fit
       */
      const auto sites = GraphAlgorithms::sites(part.graph, v);
      const unsigned S = sites.size();
      for(unsigned siteIndex = 0; siteIndex < S; ++siteIndex) {
        const auto& site = sites.at(siteIndex);
        const unsigned siteSize = site.size();
        if(siteSize == 1) {
          continue;
        }

        const auto geometry = hapticPlaneGeometry(part.angstromPositions, v, site);

        // Less than 30° and a good plane fit indicate the haptic site is fine
        if(geometry.angle < M_PI / 6 && geometry.rmsd < 0.2) {
          continue;
        }

        if(siteSize == 2) {
          // Suggest the vertex further from the center
          const double frontDistance = (
            part.angstromPositions.at(v)
            - part.angstromPositions.at(site.front())
          ).norm();
          const double backDistance = (
            part.angstromPositions.at(v)
            - part.angstromPositions.at(site.back())
          ).norm();
          const AtomIndex toRemove = frontDistance < backDistance ? site.back() : site.front();
          addFalsePositive(
            parts.componentMap.invert(
              ComponentMap::ComponentIndexPair {component, v}
            ),
            parts.componentMap.invert(
              ComponentMap::ComponentIndexPair {component, toRemove}
            ),
            geometry.angle * 2 / M_PI
          );
        } else {
          const auto suggestedRemovalOption = bestRemovalFromHapticSite(
            part.graph,
            part.angstromPositions,
            v,
            sites,
            siteIndex
          );
          if(suggestedRemovalOption) {
            for(const AtomIndex w : suggestedRemovalOption->removals) {
              addFalsePositive(
                parts.componentMap.invert(
                  ComponentMap::ComponentIndexPair {component, v}
                ),
                parts.componentMap.invert(
                  ComponentMap::ComponentIndexPair {component, w}
                ),
                suggestedRemovalOption->certainty
              );
            }
          }
        }
      }
    }
  }
  return falsePositives;
}

Utils::BondOrderCollection removeFalsePositives(
  const Utils::AtomCollection& atoms,
  Utils::BondOrderCollection bonds
) {
  // First do bad haptic bond orders
  auto haptics = badHapticLigandBonds(atoms, bonds);
  while(!haptics.empty()) {
    Temple::sort(haptics);
    FalsePositive& mostLikely = haptics.back();
    bonds.setOrder(mostLikely.i, mostLikely.j, 0.0);
    haptics = badHapticLigandBonds(atoms, bonds);
  }

  // Then do uncertain bonds
  auto uncertains = uncertainBonds(atoms, bonds);
  while(!uncertains.empty()) {
    Temple::sort(uncertains);
    FalsePositive& mostLikely = uncertains.back();
    bonds.setOrder(mostLikely.i, mostLikely.j, 0.0);
    uncertains = uncertainBonds(atoms, bonds);
  }

  return bonds;
}

} // namespace Interpret
} // namespace Molassembler
} // namespace Scine


#include <sstream>
#include <iostream>
#include <vector>
#include <limits>

#include <boost/program_options.hpp>
#include <boost/program_options/variables_map.hpp>

#include "filelib.h"
#include "stringlib.h"
#include "weights.h"
#include "hg_io.h"
#include "kbest.h"
#include "viterbi.h"
#include "ns.h"
#include "ns_docscorer.h"
#include "candidate_set.h"

using namespace std;
namespace po = boost::program_options;

void InitCommandLine(int argc, char** argv, po::variables_map* conf) {
  po::options_description opts("Configuration options");
  opts.add_options()
        ("reference,r",po::value<vector<string> >(), "[REQD] Reference translation (tokenized text)")
        ("weights,w",po::value<string>(), "[REQD] Weights files from current iterations")
        ("input,i",po::value<string>()->default_value("-"), "Input file to map (- is STDIN)")
        ("evaluation_metric,m",po::value<string>()->default_value("IBM_BLEU"), "Evaluation metric (ibm_bleu, koehn_bleu, nist_bleu, ter, meteor, etc.)")
        ("kbest_repository,R",po::value<string>(), "Accumulate k-best lists from previous iterations (parameter is path to repository)")
        ("kbest_size,k",po::value<unsigned>()->default_value(500u), "Top k-hypotheses to extract")
        ("cccp_iterations,I", po::value<unsigned>()->default_value(10u), "CCCP iterations (T')")
        ("ssd_iterations,J", po::value<unsigned>()->default_value(5u), "Stochastic subgradient iterations (T'')")
        ("eta", po::value<double>()->default_value(1e-4), "Step size")
        ("regularization_strength,C", po::value<double>()->default_value(1.0), "L2 regularization strength")
        ("alpha,a", po::value<double>()->default_value(10.0), "Cost scale (alpha); alpha * [1-metric(y,y')]")
        ("help,h", "Help");
  po::options_description dcmdline_options;
  dcmdline_options.add(opts);
  po::store(parse_command_line(argc, argv, dcmdline_options), *conf);
  bool flag = false;
  if (!conf->count("reference")) {
    cerr << "Please specify one or more references using -r <REF.TXT>\n";
    flag = true;
  }
  if (!conf->count("weights")) {
    cerr << "Please specify weights using -w <WEIGHTS.TXT>\n";
    flag = true;
  }
  if (flag || conf->count("help")) {
    cerr << dcmdline_options << endl;
    exit(1);
  }
}

struct GainFunction {
  explicit GainFunction(const EvaluationMetric* m) : metric(m) {}
  float operator()(const SufficientStats& eval_feats) const {
    float g = metric->ComputeScore(eval_feats);
    if (!metric->IsErrorMetric()) g = 1 - g;
    return g;
  }
  const EvaluationMetric* metric;
};

template <typename GainFunc>
void CostAugmentedSearch(const GainFunc& gain,
                         const training::CandidateSet& cs,
                         const SparseVector<double>& w,
                         double alpha,
                         SparseVector<double>* fmap) {
  unsigned best_i = 0;
  double best = -numeric_limits<double>::infinity();
  for (unsigned i = 0; i < cs.size(); ++i) {
    double s = cs[i].fmap.dot(w) + alpha * gain(cs[i].eval_feats);
    if (s > best) {
      best = s;
      best_i = i;
    }
  }
  *fmap = cs[best_i].fmap;
}

// runs lines 4--15 of rampion algorithm
int main(int argc, char** argv) {
  po::variables_map conf;
  InitCommandLine(argc, argv, &conf);
  const string evaluation_metric = conf["evaluation_metric"].as<string>();

  EvaluationMetric* metric = EvaluationMetric::Instance(evaluation_metric);
  DocumentScorer ds(metric, conf["reference"].as<vector<string> >());
  cerr << "Loaded " << ds.size() << " references for scoring with " << evaluation_metric << endl;
  double goodsign = -1;
  double badsign = -goodsign;

  Hypergraph hg;
  string last_file;
  ReadFile in_read(conf["input"].as<string>());
  string kbest_repo;
  if (conf.count("kbest_repository")) {
    kbest_repo = conf["kbest_repository"].as<string>();
    MkDirP(kbest_repo);
  }
  istream &in=*in_read.stream();
  const unsigned kbest_size = conf["kbest_size"].as<unsigned>();
  const unsigned tp = conf["cccp_iterations"].as<unsigned>();
  const unsigned tpp = conf["ssd_iterations"].as<unsigned>();
  const double eta = conf["eta"].as<double>();
  const double reg = conf["regularization_strength"].as<double>();
  const double alpha = conf["alpha"].as<double>();
  SparseVector<weight_t> weights;
  {
    vector<weight_t> vweights;
    const string weightsf = conf["weights"].as<string>();
    Weights::InitFromFile(weightsf, &vweights);
    Weights::InitSparseVector(vweights, &weights);
  }
  string line, file;
  vector<training::CandidateSet> kis;
  cerr << "Loading hypergraphs...\n";
  while(getline(in, line)) {
    istringstream is(line);
    int sent_id;
    kis.resize(kis.size() + 1);
    training::CandidateSet& curkbest = kis.back();
    string kbest_file;
    if (kbest_repo.size()) {
      ostringstream os;
      os << kbest_repo << "/kbest." << sent_id << ".txt.gz";
      kbest_file = os.str();
      if (FileExists(kbest_file))
        curkbest.ReadFromFile(kbest_file);
    }
    is >> file >> sent_id;
    ReadFile rf(file);
    if (kis.size() % 5 == 0) { cerr << '.'; }
    if (kis.size() % 200 == 0) { cerr << " [" << kis.size() << "]\n"; }
    HypergraphIO::ReadFromJSON(rf.stream(), &hg);
    hg.Reweight(weights);
    curkbest.AddKBestCandidates(hg, kbest_size, ds[sent_id]);
    if (kbest_file.size())
      curkbest.WriteToFile(kbest_file);
  }
  cerr << "\nHypergraphs loaded.\n";

  vector<SparseVector<weight_t> > goals(kis.size());  // f(x_i,y+,h+)
  SparseVector<weight_t> fear;  // f(x,y-,h-)
  const GainFunction gain(metric);
  for (unsigned iterp = 1; iterp <= tp; ++iterp) {
    cerr << "CCCP Iteration " << iterp << endl;
    for (unsigned i = 0; i < goals.size(); ++i)
      CostAugmentedSearch(gain, kis[i], weights, goodsign * alpha, &goals[i]);
    for (unsigned iterpp = 1; iterpp <= tpp; ++iterpp) {
      cerr << "  SSD Iteration " << iterpp << endl;
      for (unsigned i = 0; i < goals.size(); ++i) {
        CostAugmentedSearch(gain, kis[i], weights, badsign * alpha, &fear);
        weights -= weights * (eta * reg / goals.size());
        weights += (goals[i] - fear) * eta;
      }
    }
  }
  vector<weight_t> w;
  weights.init_vector(&w);
  Weights::WriteToFile("-", w);
  return 0;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================

#ifndef BOOST_SIMD_FUNCTION_SCALAR_REM_PIO2_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_REM_PIO2_HPP_INCLUDED

#include <boost/simd/function/definition/rem_pio2.hpp>
#include <boost/simd/arch/common/scalar/function/rem_pio2.hpp>

#endif


#include "magnetometer/calibration/calibrator.h"

#include <ros/node_handle.h>

#include "common/calibration/variables_center.h"
#include "common/calibration/variables_radius.h"
#include "magnetometer/calibration/variables_rotation.h"
#include "magnetometer/calibration/cost_term.h"

#include <fstream>
#include <boost/property_tree/json_parser.hpp>

using namespace magnetometer;

// CONSTRUCTORS
calibrator::calibrator(std::shared_ptr<magnetometer::data_interface>& data_interface)
{
    // Initialize true field strength.
    calibrator::m_true_field_strength = 0;

    // Store data interface.
    calibrator::m_data_interface = data_interface;

    // Initialize fit.
    calibrator::m_fit_center.setZero();
    calibrator::m_fit_radius.setZero();
    calibrator::m_fit_rotation.setZero();

    // Initialize calibration.
    calibrator::m_calibration.setIdentity();

    // Initialize thread.
    calibrator::m_running = false;
}
calibrator::~calibrator()
{
    if(calibrator::m_running)
    {
        calibrator::m_thread.join();
    }
}

// METHODS
bool calibrator::start(double true_field_strength)
{
    // Check if thread is running.
    if(!calibrator::m_running)
    {
        // Store true field strength.
        calibrator::m_true_field_strength = true_field_strength;

        // Reset fit.
        calibrator::m_fit_center.setZero();
        calibrator::m_fit_radius.setZero();
        calibrator::m_fit_rotation.setZero();

        // Reset calibration.
        calibrator::m_calibration.setIdentity();

        // Start optimization thread to generate fit.
        calibrator::m_thread = boost::thread(&calibrator::thread_worker, this);
        return true;
    }
    else
    {
        return false;
    }
}

// RESULTS
void calibrator::get_fit(Eigen::Vector3d& center, Eigen::Vector3d& radius, Eigen::Vector3d& rotation)
{
    // Return ellipsoid parameters.
    center = calibrator::m_fit_center;
    radius = calibrator::m_fit_radius;
    rotation = calibrator::m_fit_rotation;
}
void calibrator::get_calibration(Eigen::Matrix4d& calibration)
{
    calibration = calibrator::m_calibration;
}

// CALIBRATION SAVING
std::string calibrator::print_calibration()
{
    // Write to string.
    std::stringstream output;
    output << "calibration units: tesla (T)" << std::endl << std::endl;
    output << std::setprecision(10) << std::fixed;
    output << calibrator::m_calibration;

    return output.str();
}
bool calibrator::save_calibration_json(std::string filepath)
{
    // Use boost property tree to build up JSON file.
    boost::property_tree::ptree json_file;

    // Write as row-stepped array.
    boost::property_tree::ptree json_calibration;
    for(uint8_t i = 0; i < calibrator::m_calibration.rows(); ++i)
    {
        for(uint8_t j = 0; j < calibrator::m_calibration.cols(); ++j)
        {
            std::stringstream ss;
            ss << std::setprecision(10) << std::fixed << calibrator::m_calibration(i,j);
            json_calibration.push_back(boost::property_tree::ptree::value_type("", ss.str()));
        }
    }

    // Add component to JSON root.
    json_file.add_child("calibration", json_calibration);

    // Write JSON file.
    try
    {
        boost::property_tree::write_json(filepath, json_file);
    }
    catch(const std::exception& e)
    {
        ROS_ERROR_STREAM("error writing JSON calibration file (" << e.what() << ")");
        return false;
    }

    return true;
}
bool calibrator::save_calibration_yaml(std::string filepath)
{
    // Create stream for writing to file.
    std::ofstream yaml_file(filepath.c_str());

    if(!yaml_file)
    {
        ROS_ERROR_STREAM("error writing YAML calibration file (" << std::strerror(errno) << ")");
        return false;
    }

    // Write transformation component.
    yaml_file << "calibration: [";
    yaml_file << std::setprecision(10) << std::fixed;
    for(uint8_t i = 0; i < calibrator::m_calibration.rows(); ++i)
    {
        for(uint8_t j = 0; j < calibrator::m_calibration.cols(); ++j)
        {
            yaml_file << calibrator::m_calibration(i,j);
            if((i+1)*(j+1) < 16)
            {
                yaml_file << ", ";
            }
        }
    }
    yaml_file << "]";

    yaml_file.close();

    return true;
}

// THREADING
void calibrator::thread_worker()
{
    // Flag thread as running.
    calibrator::m_running = true;

    // Create a local copy of points to fit from the data interface.
    std::shared_ptr<std::vector<Eigen::Vector3d>> data_points = std::make_shared<std::vector<Eigen::Vector3d>>();

    // Copy the points locally and scale them.
    data_points->clear();
    Eigen::Vector3d new_point;
    for(uint32_t i = 0; i < calibrator::m_data_interface->n_points(); ++i)
    {
        // Grab point from data interface.
        calibrator::m_data_interface->get_point(i, new_point);

        // Scale point.
        new_point *= calibrator::m_field_scale;

        // Add point.
        data_points->push_back(new_point);
    }

    // Calculate the bounding box and it's parameters.
    Eigen::Vector3d bb_min, bb_max;
    bb_min.fill(std::numeric_limits<double>::infinity());
    bb_max.fill(-std::numeric_limits<double>::infinity());
    for(auto point = data_points->cbegin(); point != data_points->cend(); ++point)
    {
        for(uint8_t i = 0; i < 3; ++i)
        {
            if((*point)(i) < bb_min(i))
            {
                bb_min(i) = (*point)(i);
            }
            if((*point)(i) > bb_max(i))
            {
                bb_max(i) = (*point)(i);
            }
        }
    }
    // Calculate bounding box center.
    Eigen::Vector3d bb_width = bb_max - bb_min;
    Eigen::Vector3d bb_radius = bb_width / 2.0;
    Eigen::Vector3d bb_center = bb_max + bb_min;
    bb_center /= 2.0;

    // Set up the optimization components.
    std::shared_ptr<ifopt::variables_center> variables_center = std::make_shared<ifopt::variables_center>();
    std::shared_ptr<ifopt::variables_rotation> variables_rotation = std::make_shared<ifopt::variables_rotation>();
    std::shared_ptr<ifopt::variables_radius> variables_radius = std::make_shared<ifopt::variables_radius>();
    std::shared_ptr<ifopt::cost_term> cost_term = std::make_shared<ifopt::cost_term>(data_points);

    // Set initial guess and range for each variable.
    // Center
    variables_center->SetVariables(bb_center);
    variables_center->set_range(bb_center - bb_width*0.25, bb_center + bb_width*0.25);
    // Radius
    variables_radius->SetVariables(bb_radius);
    variables_radius->set_range(bb_radius * 0.5, bb_radius * 1.5);
    // Rotation
    variables_rotation->set_max(M_PI);

    // Set component parameters.
    cost_term->p_gradient_perturbation(0.000001);

    // Set up the optimization problem.
    ifopt::Problem problem;
    problem.AddVariableSet(variables_center);
    problem.AddVariableSet(variables_rotation);
    problem.AddVariableSet(variables_radius);
    problem.AddCostSet(cost_term);

    // Run solver.
    ifopt::IpoptSolver solver;
    solver.SetOption("max_cpu_time", 30.0);
    solver.Solve(problem);

    // Check solve status.
    bool solved = solver.GetReturnStatus() == 0;

    // If solved, calculate calibration.
    if(solved)
    {
        // Extract and scale fit parameters.
        calibrator::m_fit_center = variables_center->GetValues() / calibrator::m_field_scale;
        calibrator::m_fit_radius = variables_radius->GetValues() / calibrator::m_field_scale;
        calibrator::m_fit_rotation = variables_rotation->GetValues();

        // Build up a fit ellipse.
        common::ellipsoid fit;
        fit.set_center(calibrator::m_fit_center);
        fit.set_radius(calibrator::m_fit_radius);
        fit.set_rotation(calibrator::m_fit_rotation);

        // Calculate offset vector.
        Eigen::Vector3d center;
        fit.get_center(center);
        Eigen::Matrix4d h_offset;
        h_offset.setIdentity();
        h_offset.block(0, 3, 3, 1) = -center;

        // Calculate scale transform.
        Eigen::Vector3d fit_radius;
        fit.get_radius(fit_radius);
        Eigen::Matrix4d h_scale;
        h_scale.setIdentity();
        h_scale(0,0) = calibrator::m_true_field_strength / fit_radius(0);
        h_scale(1,1) = calibrator::m_true_field_strength / fit_radius(1);
        h_scale(2,2) = calibrator::m_true_field_strength / fit_radius(2);

        // Get rotation and counter-rotation transformations.
        Eigen::Matrix3d rotation, rotation_t;
        fit.get_r(rotation);
        fit.get_rt(rotation_t);
        // Turn into homogeneous transforms.
        Eigen::Matrix4d h_rotation, h_rotation_t;
        h_rotation.setIdentity();
        h_rotation_t.setIdentity();
        h_rotation.block(0, 0, 3, 3) = rotation;
        h_rotation_t.block(0, 0, 3, 3) = rotation_t;

        // Calculate and set calibration transform.
        calibrator::m_calibration.noalias() = h_rotation * h_scale * h_rotation_t * h_offset;
    }

    // Signal completion.
    calibrator::calibration_completed(solved);

    // Flag thread as finished.
    calibrator::m_running = false;
}


/*--------------------------------------------------------------------------------------
 Ethanon Engine (C) Copyright 2008-2013 Andre Santee
 http://ethanonengine.com/

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

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

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

#include "ETHScriptWrapper.h"

#include <boost/shared_ptr.hpp>

#include <math.h>

#if defined(_MSC_VER)
	#pragma warning (disable : 4512)
#endif

#include <hashlibpp.h>

str_type::string ETHScriptWrapper::Math::GetHashFromString(const str_type::string& type, const str_type::string& str)
{
	wrapperfactory factory;
	boost::shared_ptr<hashwrapper> wrapper(factory.create(type));
	return wrapper->getHashFromString(str);
}

str_type::string ETHScriptWrapper::Math::GetMD5HashFromString(const str_type::string& str)
{
	return GetHashFromString(GS_L("MD5"), str);
}

str_type::string ETHScriptWrapper::Math::GetSHA1HashFromString(const str_type::string& str)
{
	return GetHashFromString(GS_L("SHA1"), str);
}

float ETHScriptWrapper::Math::Length(const Vector3& v)
{
	return v.Length();
}

float ETHScriptWrapper::Math::Length(const Vector2& v)
{
	return v.Length();
}

float ETHScriptWrapper::Math::Sign(const float v)
{
	return gs2d::math::Sign(v);
}

int ETHScriptWrapper::Math::Sign(const int v)
{
	return gs2d::math::Sign(v);
}

float ETHScriptWrapper::Math::Distance(const Vector2& a, const Vector2& b)
{
	return Length(a - b);
}

float ETHScriptWrapper::Math::Distance(const Vector3& a, const Vector3& b)
{
	return Length(a - b);
}


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/stl.h>
#include <Eigen/Dense>
#include <unordered_map>
#include <map>
#include <set>
#include <vector>

using MatrixXi = Eigen::Matrix<long, Eigen::Dynamic, Eigen::Dynamic, Eigen::ColMajor>;
using VectorXi = Eigen::Matrix<long, Eigen::Dynamic, 1>;
using MatrixXiRM = Eigen::Matrix<long, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>;
using namespace std;

const Eigen::Ref<const MatrixXi> default_mat = MatrixXi(0,3);
const Eigen::Ref<const MatrixXi> default_vec = VectorXi(0);

MatrixXiRM listdiff(
	const Eigen::Ref<const MatrixXi> &list_to_filter,
	const Eigen::Ref<const MatrixXi> &ground_truth,
	const Eigen::Ref<const MatrixXi> &complement = default_mat,
	const Eigen::Ref<const VectorXi> &extra_pids = default_vec,
	int cut_to_size = 0
	) {
	assert(list_to_filter.cols() == 3);
	assert(ground_truth.cols() == 3);
	assert(complement.cols() == 3);

	unordered_map<long, unordered_set<long>> ground_truth_map;
	for(uint i=0; i<ground_truth.rows(); i++){
		long pid = ground_truth(i,0);
		long song_id = ground_truth(i,1);
		ground_truth_map[pid].insert(song_id);
	}

	unordered_map<long, map<long, long>> filtered_lists;
	unordered_map<long, unordered_set<long>> filtered_lists_sets;
	for(uint i=0; i<list_to_filter.rows(); i++){
		long pid = list_to_filter(i,0);
		long song_id = list_to_filter(i,1);
		long pos = list_to_filter(i,2);
		auto gtset_it = ground_truth_map.find(pid);
		if(gtset_it == ground_truth_map.end() || gtset_it->second.find(song_id) == gtset_it->second.end()){
			filtered_lists[pid][pos] = song_id;
			filtered_lists_sets[pid].insert(song_id);
		}
	}
	for(uint i=0; i<extra_pids.rows(); i++){
		long pid = extra_pids(i);
		if(filtered_lists.find(pid) == filtered_lists.end()){
			filtered_lists[pid] = map<long, long>();
			filtered_lists_sets[pid] = unordered_set<long>();
		}
	}

	unordered_map<long, vector<long>> complement_vectors;
	for(uint i=0; i<complement.rows(); i++){
		long pid = complement(i,0);
		long song_id = complement(i,1);
		long pos = complement(i,2);
		vector<long> &comp_vec = complement_vectors[pid];
		if((long)comp_vec.size() < pos){
			comp_vec.resize(pos);
		}
		comp_vec[pos-1] = song_id;
	}
	vector<long> output_collector;
	for(auto pid_it = filtered_lists.begin(); pid_it != filtered_lists.end(); ++pid_it){
		long next_pos = 1;
		for(auto song_it = pid_it->second.begin(); song_it != pid_it->second.end(); ++song_it){
			output_collector.push_back(pid_it->first);
			output_collector.push_back(song_it->second);
			output_collector.push_back(next_pos);
			next_pos++;
			if(cut_to_size != 0 && next_pos > cut_to_size){
				break;
			}
		}

		if(complement.rows() != 0 && (cut_to_size == 0 || next_pos <= cut_to_size)){
			auto comp_ref = complement_vectors.find(pid_it->first);
			if(comp_ref == complement_vectors.end()){
				comp_ref = complement_vectors.find(-1);
			}
			if(comp_ref == complement_vectors.end()){
				continue;
			}
			auto gtset = ground_truth_map.find(pid_it->first);
			auto flset = filtered_lists_sets.find(pid_it->first);
			for(auto comp_it = comp_ref->second.begin(); comp_it != comp_ref->second.end(); ++comp_it){
				bool in_gt = gtset != ground_truth_map.end() && gtset->second.find(*comp_it) != gtset->second.end();
				bool in_list = flset != ground_truth_map.end() && flset->second.find(*comp_it) != flset->second.end();

				if(in_gt || in_list) {
					continue;
				}
				output_collector.push_back(pid_it->first);
				output_collector.push_back(*comp_it);
				output_collector.push_back(next_pos);
				next_pos++;
				if(cut_to_size != 0 && next_pos > cut_to_size){
					break;
				}
			}
		}
	}

    return Eigen::Map<MatrixXiRM>(output_collector.data(), output_collector.size() / 3, 3);
}

namespace py = pybind11;

PYBIND11_MODULE(listdiff, m) {
	m.def(
		"listdiff",
		[&default_mat, &default_vec](
			const Eigen::Ref<const MatrixXi> &list_to_filter,
			const Eigen::Ref<const MatrixXi> &ground_truth,
			const Eigen::Ref<const MatrixXi> &complement = default_mat,
			const Eigen::Ref<const VectorXi> &extra_pids = default_vec,
			int cut_to_size = 0
		) -> MatrixXiRM {
			return listdiff(list_to_filter, ground_truth, complement, extra_pids, cut_to_size);
		},
		py::arg().noconvert(),
		py::arg().noconvert(),
		py::arg("complement").noconvert() = default_mat,
		py::arg("extra_pids").noconvert() = default_vec,
		py::arg("cut_to_size") = 0
	);

#ifdef VERSION_INFO
    m.attr("__version__") = VERSION_INFO;
#else
    m.attr("__version__") = "dev";
#endif
}


#include <boost/test/unit_test.hpp>

#include <scorum/chain/schema/account_objects.hpp>
#include <scorum/chain/schema/budget_objects.hpp>

#include <scorum/chain/services/account.hpp>
#include <scorum/chain/services/budgets.hpp>
#include <scorum/chain/services/dynamic_global_property.hpp>
#include <scorum/chain/services/reward_balancer.hpp>
#include <scorum/rewards_math/formulas.hpp>

#include "database_blog_integration.hpp"
#include "actor.hpp"

namespace active_sp_holders_reward_tests {

using namespace database_fixture;

class active_sp_holders_reward_fixture : public database_blog_integration_fixture
{
public:
    active_sp_holders_reward_fixture()
        : budget_service(db.fund_budget_service())
        , account_service(db.account_service())
        , dprops_service(db.obtain_service<dbs_dynamic_global_property>())
        , voters_reward_sp_service(db.obtain_service<dbs_voters_reward_sp>())
        , voters_reward_scr_service(db.obtain_service<dbs_voters_reward_scr>())
        , alice("alice")
        , bob("bob")
    {
        alice.percent(70);
        bob.percent(30);

        asset founders_supply = ASSET_SP(100000000);

        genesis_state_type genesis = database_integration_fixture::default_genesis_state()
                                         .accounts(alice, bob)
                                         .founders_supply(founders_supply)
                                         .founders(alice, bob)
                                         .generate();

        open_database(genesis);

        db.post_apply_operation.connect([&](const operation_notification& note) { on_operation(note.op); });
    }

    virtual void open_database_impl(const genesis_state_type& genesis) override
    {
        database_integration_fixture::open_database_impl(genesis);

        db.set_hardfork(SCORUM_NUM_HARDFORKS);
    }

    inline asset get_active_voters_reward(const asset& total)
    {
        return total * SCORUM_ACTIVE_SP_HOLDERS_PER_BLOCK_REWARD_PERCENT / SCORUM_100_PERCENT;
    }

    fund_budget_service_i& budget_service;
    account_service_i& account_service;
    dynamic_global_property_service_i& dprops_service;
    voters_reward_sp_service_i& voters_reward_sp_service;
    voters_reward_scr_service_i& voters_reward_scr_service;

    Actor alice;
    Actor bob;

    void set_voter(const Actor& voter)
    {
        _voter = voter;
    }

    int op_times() const
    {
        return _op_times;
    }

private:
    void on_operation(const operation& op)
    {
        op.weak_visit([&](const active_sp_holders_reward_operation& o) {
            if (_voter.name != account_name_type())
            {
                BOOST_REQUIRE_EQUAL(o.sp_holder, bob.name);
            }
            ++_op_times;
        });
    }

    Actor _voter;
    int _op_times = 0;
};

using namespace scorum::chain;
using namespace scorum::protocol;

BOOST_FIXTURE_TEST_SUITE(active_sp_holders_reward_tests, active_sp_holders_reward_fixture)

SCORUM_TEST_CASE(per_block_sp_payment_from_fund_budget)
{
    asset bob_sp_before = account_service.get_account(bob.name).scorumpower;

    auto post = create_post(alice).push();
    post.vote(bob).in_block();

    auto initial_blocks = db.head_block_num();
    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);
    auto pass_blocks = db.head_block_num() - initial_blocks;

    auto active_sp_holders_reward = get_active_voters_reward(budget_service.get().per_block);
    active_sp_holders_reward *= pass_blocks;

    BOOST_REQUIRE_EQUAL(account_service.get_account(bob.name).scorumpower, bob_sp_before + active_sp_holders_reward);
}

SCORUM_TEST_CASE(per_block_sp_payment_from_fund_budget_if_no_active_voters_exist)
{
    generate_block();

    auto active_sp_holders_reward = get_active_voters_reward(budget_service.get().per_block);

    auto& balancer = voters_reward_sp_service.get();

    BOOST_REQUIRE_EQUAL(balancer.balance, active_sp_holders_reward);
}

SCORUM_TEST_CASE(per_block_sp_payment_division_from_fund_budget)
{
    asset alice_sp_before = account_service.get_account(alice.name).scorumpower;
    asset bob_sp_before = account_service.get_account(bob.name).scorumpower;

    auto post = create_post(alice).push();
    post.vote(bob).push();
    post.vote(alice).in_block();

    auto initial_blocks = db.head_block_num();
    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);
    auto pass_blocks = db.head_block_num() - initial_blocks;

    auto active_sp_holders_reward = get_active_voters_reward(budget_service.get().per_block);
    active_sp_holders_reward *= pass_blocks;

    auto alice_reward = active_sp_holders_reward * alice.sp_percent / 100;
    auto bob_reward = active_sp_holders_reward - alice_reward;

    BOOST_REQUIRE_EQUAL(account_service.get_account(alice.name).scorumpower, alice_sp_before + alice_reward);

    BOOST_REQUIRE_EQUAL(account_service.get_account(bob.name).scorumpower, bob_sp_before + bob_reward);
}

SCORUM_TEST_CASE(per_block_payments_are_stopped_after_battary_restored)
{
    const auto& voter = account_service.get_account(bob.name);
    asset bob_sp_before = voter.scorumpower;

    const auto vote_weight = SCORUM_PERCENT(100);

    uint16_t current_power = scorum::rewards_math::calculate_restoring_power(
        voter.voting_power, dprops_service.head_block_time(), voter.last_vote_time, SCORUM_VOTE_REGENERATION_SECONDS);

    uint16_t used_power
        = scorum::rewards_math::calculate_used_power(current_power, vote_weight, SCORUM_VOTING_POWER_DECAY_PERCENT);

    auto voting_power_restoring_time = scorum::rewards_math::calculate_expected_restoring_time(
        current_power - used_power, dprops_service.head_block_time(), SCORUM_VOTE_REGENERATION_SECONDS);

    auto post = create_post(alice).push();
    post.vote(bob, vote_weight).in_block();

    auto initial_blocks = db.head_block_num();
    generate_blocks(voting_power_restoring_time + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);
    auto pass_blocks = db.head_block_num() - initial_blocks;

    auto active_sp_holders_reward = get_active_voters_reward(budget_service.get().per_block);

    BOOST_REQUIRE_EQUAL(voter.scorumpower, bob_sp_before + active_sp_holders_reward * pass_blocks);

    initial_blocks = db.head_block_num();
    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);
    pass_blocks = db.head_block_num() - initial_blocks;

    BOOST_REQUIRE_EQUAL(voter.scorumpower, bob_sp_before + active_sp_holders_reward * pass_blocks);

    auto& balancer = voters_reward_sp_service.get();

    pass_blocks += 1; // in_block

    BOOST_REQUIRE_EQUAL(balancer.balance, active_sp_holders_reward * pass_blocks);
}

SCORUM_TEST_CASE(payments_from_sp_balancer_arter_fund_budget_is_over)
{
    const auto fund_budget_period_in_blocks = 2;

    auto& fund_budget = budget_service.get();

    budget_service.update(fund_budget, [&](fund_budget_object& b) {
        b.per_block = b.balance / fund_budget_period_in_blocks;
        b.deadline = dprops_service.head_block_time() + fund_budget_period_in_blocks * SCORUM_BLOCK_INTERVAL;
    });

    generate_blocks(fund_budget_period_in_blocks);

    BOOST_REQUIRE(!budget_service.is_exists());

    const auto& voter = account_service.get_account(bob.name);
    asset bob_sp_before = voter.scorumpower;

    auto post = create_post(alice).push();
    post.vote(bob).in_block();

    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    auto& balancer = voters_reward_sp_service.get();

    BOOST_REQUIRE(balancer.current_per_block_reward.amount != 0);

    BOOST_REQUIRE_GT(voter.scorumpower, bob_sp_before);
}

SCORUM_TEST_CASE(active_sp_holders_op_notifications_check)
{
    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    set_voter(bob);

    auto post = create_post(alice).push();
    post.vote(bob, SCORUM_PERCENT(30)).in_block();

    BOOST_CHECK_EQUAL(op_times(), 0);

    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    BOOST_CHECK_EQUAL(op_times(), 1);

    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    BOOST_CHECK_EQUAL(op_times(), 1);

    post.vote(bob, SCORUM_PERCENT(90)).in_block();

    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    BOOST_CHECK_EQUAL(op_times(), 2);
}

SCORUM_TEST_CASE(second_cashout_if_no_vote_after_first_cashout_check)
{
    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    set_voter(bob);

    auto start = db.head_block_time();

    auto post = create_post(alice).push();
    post.vote(bob).in_block();

    generate_blocks(start
                    + fc::seconds(SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD.to_seconds() - SCORUM_BLOCK_INTERVAL * 2));

    post = create_post(bob).push();
    post.vote(bob).in_block();

    BOOST_CHECK_EQUAL(op_times(), 0);

    generate_blocks(start + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    const auto& bob_obj = account_service.get_account(bob.name);
    BOOST_REQUIRE_GT(bob_obj.voting_power_restoring_time.to_iso_string(), db.head_block_time().to_iso_string());
    BOOST_REQUIRE_LT(bob_obj.voting_power, SCORUM_100_PERCENT);

    BOOST_CHECK_NE(bob_obj.active_sp_holders_cashout_time.to_iso_string(),
                   fc::time_point_sec::maximum().to_iso_string());

    BOOST_CHECK_EQUAL(op_times(), 1);

    generate_blocks(db.head_block_time() + SCORUM_ACTIVE_SP_HOLDERS_REWARD_PERIOD);

    BOOST_CHECK_EQUAL(bob_obj.active_sp_holders_cashout_time.to_iso_string(),
                      fc::time_point_sec::maximum().to_iso_string());

    BOOST_CHECK_EQUAL(op_times(), 2);
}

BOOST_AUTO_TEST_SUITE_END()
}


/*!
 * @file
 * Contains unit tests for the @ref Monad typeclass.
 */

#include <boost/mpl11/monad.hpp>

#include <boost/mpl11/core.hpp>
#include <boost/mpl11/functor.hpp>

#include "test_method_dispatch.hpp"

using namespace boost::mpl11;

///////////////////////////
// Test method dispatching
///////////////////////////
namespace boost { namespace mpl11 {
    template <>
    struct Functor<Archetype<>>
        : instantiate<Functor>::with<Archetype<>>
    { };

    template <>
    struct Monad<Archetype<>> : instantiate<Monad>::with<Archetype<>> {
        template <typename> using join_impl = method_tag<join>;
        template <typename> using unit_impl = method_tag<unit>;
    };
}}

struct tests
    : method<join, archetype<>>
    , method<unit, Archetype<>, undefined>
{ };

int main() { }


/* =============================================================================== */
/* Dynamic Generic Programming                                                     */
/* =============================================================================== */
/* Test project using the Boost.TypeErasure library                                */
/* (http://www.boost.org/doc/libs/1_56_0/doc/html/boost_typeerasure.html)          */
/*                                                                                */
/* Tested with Boost 1.56                                                          */
/* =============================================================================== */
/* Author: Andy Prowl                                                              */
/* =============================================================================== */

struct Point
{
    Point(double x, double y) : x{x}, y{y} { }
    friend bool operator == (Point const& lhs, Point const& rhs)
    { return (lhs.x == rhs.x) && (lhs.y == rhs.y); }
    double x = 0;
    double y = 0;
};

struct Circle
{
    Circle(Point const& center, double radius)
        : _center{center}, _radius{radius} { }
    double get_area() const { return _radius * _radius * 3.14159; }
    double get_perimeter() const { return _radius * 2 * 3.14159; }
private:
    Point _center;
    double _radius;
};

struct Rectangle
{
    Rectangle(Point const& center, double width, double height)
        : _center{center}, _width{width}, _height{height} { }
    double get_area() const { return _width * _height; }
    double get_perimeter() const { return (_width + _height) * 2; }
    void set_size(double width, double height) { _width = width; _height = height; }
private:
    Point _center;
    double _width;
    double _height;
};

#include <boost/type_erasure/any.hpp>
#include <boost/mpl/vector.hpp>
#include <boost/type_erasure/member.hpp>

namespace erasure = boost::type_erasure;

BOOST_TYPE_ERASURE_MEMBER((has_get_area), get_area)
BOOST_TYPE_ERASURE_MEMBER((has_get_perimeter), get_perimeter)

using ShapeRequirements = boost::mpl::vector<
    erasure::copy_constructible<>,
    has_get_area<double(), erasure::_self const>,
    has_get_perimeter<double(), erasure::_self const>,
    erasure::relaxed>;

using ShapeRef = erasure::any<ShapeRequirements, erasure::_self&>;

#include <vector>
#include <iostream>

int main()
{
    Rectangle r{{1.0, 2.0}, 5.0, 6.0};

    ShapeRef s{r};

    std::cout << s.get_area() << std::endl;

    r.set_size(4, 2);

    std::cout << s.get_area() << std::endl;
}


// Author: Derek Barnett

#include <pbcopper/data/Frames.h>

#include <cassert>
#include <cmath>
#include <cstddef>
#include <cstdint>

#include <algorithm>
#include <limits>
#include <mutex>
#include <ostream>
#include <type_traits>

#include <boost/algorithm/string/join.hpp>
#include <boost/range/adaptor/transformed.hpp>

namespace PacBio {
namespace Data {
namespace {

static std::vector<uint16_t> framepoints;
static std::vector<uint8_t> frameToCode;
static uint16_t maxFramepoint;
static std::mutex initIpdDownsamplingMutex;

void InitIpdDownsampling()
{
    std::lock_guard<std::mutex> lock(initIpdDownsamplingMutex);

    if (!framepoints.empty()) return;

    // liftover from Dave's python code:
    // .../bioinformatics/tools/kineticsTools/kineticsTools/_downsampling.py

    const int B = 2;
    const int t = 6;
    const double T = pow(B, t);

    int next = 0;
    double grain;
    const int end = 256 / T;
    for (int i = 0; i < end; ++i) {
        grain = pow(B, i);
        std::vector<uint16_t> nextOnes;
        for (double j = 0; j < T; ++j)
            nextOnes.push_back(j * grain + next);
        next = nextOnes.back() + grain;
        framepoints.insert(framepoints.end(), nextOnes.cbegin(), nextOnes.cend());
    }
    assert(framepoints.size() - 1 <= std::numeric_limits<uint8_t>::max());

    const uint16_t maxElement = (*max_element(framepoints.cbegin(), framepoints.cend()));
    frameToCode.assign(maxElement + 1, 0);

    const int fpEnd = framepoints.size() - 1;
    uint8_t i = 0;
    uint16_t fl = 0;
    uint16_t fu = 0;
    for (; i < fpEnd; ++i) {
        fl = framepoints[i];
        fu = framepoints[i + 1];
        if (fu > fl + 1) {
            const int middle = (fl + fu) / 2;
            for (int f = fl; f < middle; ++f)
                frameToCode[f] = i;
            for (int f = middle; f < fu; ++f)
                frameToCode[f] = i + 1;
        } else
            frameToCode[fl] = i;
    }

    // this next line differs from the python implementation (there, it's "i+1")
    // our C++ for loop has incremented our index counter one more time than the indexes from python enumerate(...)
    frameToCode[fu] = i;
    maxFramepoint = fu;
}

uint16_t CodeToFrames(const uint8_t code) { return framepoints[code]; }

std::vector<uint16_t> CodeToFrames(const std::vector<uint8_t>& codedData)
{
    InitIpdDownsampling();

    const auto length = codedData.size();
    std::vector<uint16_t> frames(length, 0);
    for (size_t i = 0; i < length; ++i)
        frames[i] = CodeToFrames(codedData[i]);
    return frames;
}

uint8_t FramesToCode(const uint16_t frame) { return frameToCode[std::min(maxFramepoint, frame)]; }

std::vector<uint8_t> FramesToCode(const std::vector<uint16_t>& frames)
{
    InitIpdDownsampling();

    const auto length = frames.size();
    std::vector<uint8_t> result(length, 0);
    for (size_t i = 0; i < length; ++i)
        result[i] = FramesToCode(frames[i]);
    return result;
}

}  // namespace

static_assert(std::is_copy_constructible<Frames>::value, "Frames(const Frames&) is not = default");
static_assert(std::is_copy_assignable<Frames>::value,
              "Frames& operator=(const Frames&) is not = default");

#ifndef __INTEL_COMPILER
static_assert(std::is_nothrow_move_constructible<Frames>::value,
              "Frames(Frames&&) is not = noexcept");
static_assert(std::is_nothrow_move_assignable<Frames>::value,
              "Frames& operator=(Frames&&) is not = noexcept");
#endif

Frames::Frames(std::vector<uint16_t> frames) noexcept : std::vector<uint16_t>{std::move(frames)} {}

const std::vector<uint16_t>& Frames::Data() const { return *this; }

std::vector<uint16_t>& Frames::DataRaw() { return *this; }

Frames Frames::Decode(const std::vector<uint8_t>& codedData) { return CodeToFrames(codedData); }

std::vector<uint8_t> Frames::Encode(const std::vector<uint16_t>& frames)
{
    return FramesToCode(frames);
}

std::vector<uint8_t> Frames::Encode() const { return Frames::Encode(*this); }

Frames& Frames::Data(std::vector<uint16_t> frames)
{
    *this = std::move(frames);
    return *this;
}

bool Frames::operator==(const Frames& other) const noexcept
{
    return static_cast<const std::vector<uint16_t>&>(*this) ==
           static_cast<const std::vector<uint16_t>&>(other);
}

bool Frames::operator!=(const Frames& other) const noexcept { return !(*this == other); }

std::ostream& operator<<(std::ostream& os, const Frames& frames)
{
    return os << "Frames("
              << boost::algorithm::join(frames | boost::adaptors::transformed(
                                                     [](uint16_t i) { return std::to_string(i); }),
                                        ", ")
              << ')';
}

}  // namespace Data
}  // namespace PacBio


/*    Copyright (c) 2010-2019, Delft University of Technology
 *    All rigths reserved
 *
 *    This file is part of the Tudat. Redistribution and use in source and
 *    binary forms, with or without modification, are permitted exclusively
 *    under the terms of the Modified BSD license. You should have received
 *    a copy of the license with this file. If not, please or visit:
 *    http://tudat.tudelft.nl/LICENSE.
 */

#define BOOST_TEST_MAIN

#include <limits>
#include <string>
#include <vector>

#include <boost/test/unit_test.hpp>
#include <boost/make_shared.hpp>
#include <boost/lambda/lambda.hpp>

#include "Tudat/Basics/testMacros.h"

#include "Tudat/InputOutput/basicInputOutput.h"
#include "Tudat/External/SpiceInterface/spiceInterface.h"

#include "Tudat/Astrodynamics/BasicAstrodynamics/sphericalBodyShapeModel.h"
#include "Tudat/Astrodynamics/Ephemerides/constantEphemeris.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationRate.h"
#include "Tudat/Astrodynamics/OrbitDetermination/EstimatableParameters/constantRotationalOrientation.h"
#include "Tudat/SimulationSetup/EstimationSetup/createCartesianStatePartials.h"
#include "Tudat/Astrodynamics/OrbitDetermination/ObservationPartials/UnitTests/numericalObservationPartial.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createGroundStations.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/createBodies.h"
#include "Tudat/SimulationSetup/EnvironmentSetup/defaultBodies.h"
#include "Tudat/SimulationSetup/EstimationSetup/createEstimatableParameters.h"

namespace tudat
{
namespace unit_tests
{

using namespace tudat::basic_astrodynamics;
using namespace tudat::ground_stations;
using namespace tudat::gravitation;
using namespace tudat::ephemerides;
using namespace tudat::observation_models;
using namespace tudat::simulation_setup;
using namespace tudat::spice_interface;
using namespace tudat::orbit_determination;
using namespace tudat::estimatable_parameters;
using namespace tudat::observation_partials;

BOOST_AUTO_TEST_SUITE( test_position_partials)

//! Test partial derivatives of positions w.r.t. parameters. Partials of most observables are computed in terms of these
//! partials
BOOST_AUTO_TEST_CASE( testCartesianStatePartials )
{
    //Load spice kernels.
    spice_interface::loadStandardSpiceKernels( );

    // Specify initial time
    double initialEphemerisTime = 1.0E7;

    // Create bodies.
    NamedBodyMap bodyMap;
    bodyMap[ "Earth" ] = std::make_shared< Body >( );
    bodyMap[ "Moon" ] = std::make_shared< Body >( );
    bodyMap[ "Sun" ] = std::make_shared< Body >( );
    bodyMap[ "Mars" ] = std::make_shared< Body >( );

    // Define properties of bodies
    bodyMap[ "Earth" ]->setShapeModel( std::make_shared< SphericalBodyShapeModel >(
                                           spice_interface::getAverageRadius( "Earth" ) ) );
    bodyMap[ "Mars" ]->setShapeModel( std::make_shared< SphericalBodyShapeModel >(
                                          spice_interface::getAverageRadius( "Mars" ) ) );

    bodyMap[ "Earth" ]->setEphemeris(
                std::make_shared< ConstantEphemeris >(
                    getBodyCartesianStateAtEpoch( "Earth", "SSB", "ECLIPJ2000", "NONE", 0.0 ),
                    "SSB", "ECLIPJ2000" ) );
    bodyMap[ "Moon" ]->setEphemeris(
                std::make_shared< ConstantEphemeris >(
                    getBodyCartesianStateAtEpoch( "Moon", "SSB", "ECLIPJ2000", "NONE", 0.0 ),
                    "SSB", "ECLIPJ2000" ) );
    bodyMap[ "Sun" ]->setEphemeris(
                std::make_shared< ConstantEphemeris >(
                    getBodyCartesianStateAtEpoch( "Sun", "SSB", "ECLIPJ2000", "NONE", 0.0 ),
                    "SSB", "ECLIPJ2000" ) );
    bodyMap[ "Mars" ]->setEphemeris(
                std::make_shared< ConstantEphemeris >(
                    getBodyCartesianStateAtEpoch( "Mars", "SSB", "ECLIPJ2000", "NONE", 0.0 ),
                    "SSB", "ECLIPJ2000" ) );

    bodyMap[ "Sun" ]->setGravityFieldModel(
                std::make_shared< GravityFieldModel >( getBodyGravitationalParameter( "Sun" ) ) );
    bodyMap[ "Moon" ]->setGravityFieldModel(
                std::make_shared< GravityFieldModel >( getBodyGravitationalParameter( "Moon" ) ) );
    bodyMap[ "Earth" ]->setGravityFieldModel(
                std::make_shared< GravityFieldModel >( getBodyGravitationalParameter( "Earth" ) ) );

    bodyMap[ "Earth" ]->setRotationalEphemeris(
                createRotationModel(
                    std::make_shared< SimpleRotationModelSettings >(
                        "ECLIPJ2000", "IAU_Earth",
                        spice_interface::computeRotationQuaternionBetweenFrames(
                            "ECLIPJ2000", "IAU_Earth", initialEphemerisTime ),
                        initialEphemerisTime, 2.0 * mathematical_constants::PI / physical_constants::JULIAN_DAY ), "Earth" ) );
    bodyMap[ "Mars" ]->setRotationalEphemeris(
                createRotationModel(
                    std::make_shared< SimpleRotationModelSettings >(
                        "ECLIPJ2000", "IAU_Mars",
                        spice_interface::computeRotationQuaternionBetweenFrames(
                            "ECLIPJ2000", "IAU_Mars", initialEphemerisTime ),
                        initialEphemerisTime, 2.0 * mathematical_constants::PI / physical_constants::JULIAN_DAY ), "Mars" ) );

    // Finalize body creation.
    setGlobalFrameBodyEphemerides( bodyMap, "SSB", "ECLIPJ2000" );

    // Create ground stations
    std::map< std::pair< std::string, std::string >, Eigen::Vector3d > groundStationsToCreate;
    groundStationsToCreate[ std::make_pair( "Earth", "Graz" ) ] =
            ( Eigen::Vector3d( ) << 1.7E6, -6.2E6, 1.3E5 ).finished( );
    groundStationsToCreate[ std::make_pair( "Mars", "MSL" ) ] =
            ( Eigen::Vector3d( ) << -2.5E5, 3.2E6, -2.65E4 ).finished( );
    createGroundStations( bodyMap, groundStationsToCreate );

    // Define list of ground station names.
    std::vector< std::pair< std::string, std::string > > groundStations;
    groundStations.push_back( std::make_pair( "Earth", "Graz" ) );
    groundStations.push_back( std::make_pair( "Mars", "MSL" ) );

    // Create link ends set.
    LinkEnds linkEnds;
    linkEnds[ observed_body ] = groundStations[ 0 ];

    LinkEnds linkEnds2;
    linkEnds2[ observed_body ] = groundStations[ 1 ];

    std::shared_ptr< GroundStation > receivingGroundStation =
            bodyMap[ "Earth" ]->getGroundStation( "Graz" );

    //Create parameter objects.
    std::shared_ptr< RotationRate > earthRotationRate = std::make_shared< RotationRate >(
                std::dynamic_pointer_cast< SimpleRotationalEphemeris >(
                    bodyMap[ "Earth" ]->getRotationalEphemeris( ) ), "Earth");
    std::shared_ptr< ConstantRotationalOrientation > earthPolePosition =
            std::make_shared< ConstantRotationalOrientation >(
                std::dynamic_pointer_cast< SimpleRotationalEphemeris >(
                    bodyMap[ "Earth" ]->getRotationalEphemeris( ) ), "Earth" );

    // Create explicit position partial objects.
    std::shared_ptr< CartesianStatePartial > partialObjectWrtReceiverPosition =
            createCartesianStatePartialsWrtBodyState( linkEnds, bodyMap, "Earth" ).begin( )->second;

    // Create explicit parameter partial objects.
    std::shared_ptr< CartesianStatePartial > partialObjectWrtReceiverRotationRate =
            createCartesianStatePartialsWrtParameter(
                linkEnds, bodyMap, earthRotationRate ).begin( )->second;
    std::shared_ptr< CartesianStatePartial > partialObjectWrtReceiverPolePosition =
            createCartesianStatePartialsWrtParameter(
                linkEnds, bodyMap, earthPolePosition ).begin( )->second;

    // Calculate transmission/reception times and states
    Eigen::Vector6d currentState;
    double receptionTime = 1.1E7;
    currentState = bodyMap.at( "Earth" )->getStateInBaseFrameFromEphemeris( receptionTime );

    double currentTime = receptionTime;

    // Compute partials
    Eigen::MatrixXd partialWrtReceiverPosition =
            partialObjectWrtReceiverPosition->calculatePartialOfPosition( currentState, currentTime );

    Eigen::MatrixXd partialWrtReceiverRotationRate =
            partialObjectWrtReceiverRotationRate->calculatePartialOfPosition( currentState, currentTime );
    Eigen::MatrixXd partialOfVelocityWrtReceiverRotationRate =
            partialObjectWrtReceiverRotationRate->calculatePartialOfVelocity( currentState, currentTime );

    Eigen::MatrixXd partialWrtReceiverPolePosition =
            partialObjectWrtReceiverPolePosition->calculatePartialOfPosition( currentState, currentTime );
    Eigen::MatrixXd partialOfVelocityWrtReceiverPolePosition =
            partialObjectWrtReceiverPolePosition->calculatePartialOfVelocity( currentState, currentTime );

    // Define observation function
    std::function< Eigen::VectorXd( const double ) > observationFunctionAtReception =
            std::bind( &Ephemeris::getCartesianState, createReferencePointEphemeris< double, double >(
                             bodyMap.at( "Earth" )->getEphemeris( ), bodyMap.at( "Earth" )->getRotationalEphemeris( ),
                             std::bind( &GroundStation::getStateInPlanetFixedFrame< double, double >,
                                          bodyMap[ "Earth" ]->getGroundStation( "Graz" ), std::placeholders::_1 ) ), std::placeholders::_1 );

    // Calculate numerical partials w.r.t. Earth state.
    Eigen::Vector3d bodyPositionVariation;
    bodyPositionVariation << 10.0, 10.0, 10.0;
    std::shared_ptr< ConstantEphemeris > earthEphemeris = std::dynamic_pointer_cast< ConstantEphemeris >(
                bodyMap[ "Earth" ]->getEphemeris( ) );
    Eigen::Vector6d earthUnperturbedState = earthEphemeris->getCartesianState( 0.0 );
    Eigen::Vector6d perturbedEarthState;
    Eigen::Matrix< double, 3, 3 > numericalPartialWrtReceiverPosition = Eigen::Matrix< double, 3, 3 >::Zero( );
    for( int i = 0; i < 3; i++ )
    {
        perturbedEarthState = earthUnperturbedState;
        perturbedEarthState( i ) += bodyPositionVariation( i );
        earthEphemeris->updateConstantState( perturbedEarthState );
        Eigen::Vector3d upPerturbedPosition = observationFunctionAtReception( currentTime ).segment( 0, 3 );

        perturbedEarthState = earthUnperturbedState;
        perturbedEarthState( i ) -= bodyPositionVariation( i );
        earthEphemeris->updateConstantState( perturbedEarthState );
        Eigen::Vector3d downPerturbedPosition = observationFunctionAtReception( currentTime ).segment( 0, 3 );

        numericalPartialWrtReceiverPosition.block( 0, i, 3, 1 ) = ( upPerturbedPosition - downPerturbedPosition ) /
                ( 2.0 * bodyPositionVariation( i ) );
    }
    earthEphemeris->updateConstantState( earthUnperturbedState );

    // Test partial w.r.t. position
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtReceiverPosition.block( 0, 0, 3, 3 ),
                                       numericalPartialWrtReceiverPosition, 1.0E-12 );

    // Compute numerical partial w.r.t. rotation rate.
    Eigen::Vector6d numericalPartialWrtReceiverRotationRate = calculateNumericalObservationParameterPartial(
                earthRotationRate, 1.0E-10, observationFunctionAtReception,
                receptionTime );

    // Test partial w.r.t. rotation rate
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialWrtReceiverRotationRate,
                                       numericalPartialWrtReceiverRotationRate.segment( 0, 3 ), 1.0E-5 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( partialOfVelocityWrtReceiverRotationRate,
                                       numericalPartialWrtReceiverRotationRate.segment( 3, 3 ), 1.0E-5 );

    // Compute numerical partial w.r.t. pole position
    Eigen::VectorXd polePositionPerturbation = ( Eigen::Vector2d( ) << 1.0E-5, 1.0E-5 ).finished( );
    Eigen::MatrixXd numericalPartialWrtReceiverPolePosition = calculateNumericalObservationParameterPartial(
                earthPolePosition, polePositionPerturbation, observationFunctionAtReception, receptionTime );

    // Test partials w.r.t. pole position (different tolernaces due to different magnitudes of partials).
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ( partialWrtReceiverPolePosition.block( 0, 0, 1, 2 ) ),
                                       ( numericalPartialWrtReceiverPolePosition.block( 0, 0, 1, 2 ) ), 1.0E-4 );
    TUDAT_CHECK_MATRIX_CLOSE_FRACTION( ( partialWrtReceiverPolePosition.block( 1, 0, 2, 2 ) ),
                                       ( numericalPartialWrtReceiverPolePosition.block( 1, 0, 2, 2 ) ), 1.0E-6 );

    for( int i = 0; i < 3; i++ )
    {
        for( int j = 0; j < 2; j++ )
        {
            BOOST_CHECK_SMALL( std::fabs( partialOfVelocityWrtReceiverPolePosition( i, j ) -
                                          numericalPartialWrtReceiverPolePosition( i + 3, j ) ), 1.0E-6 );
        }
    }
}

BOOST_AUTO_TEST_SUITE_END( )

} // namespace unit_tests

} // namespace tudat