/*
 * SPDX-FileCopyrightText: © 2018 Ambrosys GmbH
 *
 * SPDX-License-Identifier: Apache-2.0
 */

#include <Core/Common/Geometry/Helper.h>

#include <boost/geometry/strategies/spherical/distance_haversine.hpp>

namespace Core::Common::Geometry {

std::pair<Point, double> project(Point const & p, Segment const & segment)
{
    using boost::geometry::add_point;
    using boost::geometry::dot_product;
    using boost::geometry::multiply_value;
    using boost::geometry::subtract_point;

    auto const & p1 = segment.first;
    auto const & p2 = segment.second;
    auto p1p = p;
    subtract_point(p1p, p1);

    auto p1p2 = p2;
    subtract_point(p1p2, p1);

    const double segmentLength_sqred = dot_product(p1p2, p1p2);
    const double normLengthProjection = dot_product(p1p, p1p2) / segmentLength_sqred;

    auto projected = p1p2;
    multiply_value(projected, normLengthProjection);
    add_point(projected, p1);

    return {projected, normLengthProjection};
}

std::pair<Point, double> snap(Point const & p, Segment const segment)
{
    auto const result = project(p, segment);
    auto const & projected = result.first;
    auto const normLengthProjection = result.second;

    if (normLengthProjection <= 0.0)
    {
        return {segment.first, normLengthProjection};
    }
    else if (normLengthProjection >= 1.0)
    {
        return {segment.second, normLengthProjection};
    }
    else
    {
        return {projected, normLengthProjection};
    }
}

Point reverseHaversine(Point const & p, double bearing, double distanceMeter)
{
    using std::atan2;
    using std::cos;
    using std::sin;

    auto lon1 = rad(p.lon());
    auto lat1 = rad(p.lat());
    auto angdist = distanceMeter / equatorRadiusMeter;
    auto theta = rad(bearing);
    auto lat2 = asin(sin(lat1) * cos(angdist) + cos(lat1) * sin(angdist) * cos(theta));
    auto lon2 = lon1 + atan2(sin(theta) * sin(angdist) * cos(lat1), cos(angdist) - sin(lat1) * sin(lat2));
    return Point{}.setLon(degree(lon2)).setLat(degree(lat2));
}

Box buffer(Box const & box, double distanceMeter)
{
    constexpr auto sqrtOf2 = 1.414213562373095049;
    auto min_corner = reverseHaversine(box.min_corner(), 225.0, sqrtOf2 * distanceMeter);
    auto max_corner = reverseHaversine(box.max_corner(), 45.0, sqrtOf2 * distanceMeter);
    return {min_corner, max_corner};
}

ValueType geoDistance(Point const & g1, Point const & g2)
{
    auto strategy = boost::geometry::strategy::distance::haversine<double>(equatorRadiusMeter);
    return boost::geometry::distance(g1, g2, strategy);
}

std::tuple<ValueType, Point, double> geoDistance(Point const & g1, Segment const & g2)
{
    auto const clipped = snap(g1, g2);
    return {geoDistance(g1, clipped.first), clipped.first, clipped.second};
}

double heading(Point const & p1, Point const & p2)
{
    auto deltaLon = rad(p2.lon()) - rad(p1.lon());
    auto cos_p2lat = cos(rad(p2.lat()));
    auto angle = atan2(sin(deltaLon) * cos_p2lat, cos(rad(p1.lat())) * sin(rad(p2.lat())) - sin(rad(p1.lat())) * cos_p2lat * cos(deltaLon));
    return normalizeAngle(degree(angle));
}

double heading(Segment const & segment)
{
    return heading(segment.first, segment.second);
}

double normalizeAngle(double angle)
{
    angle = std::fmod(angle, 360.0);
    return angle < 0.0 ? angle + 360.0 : angle;
}

double headingDiff(double a, double b)
{
    auto res = normalizeAngle(a - b);
    return res > 180.0 ? res - 360.0 : res;
}

double absHeadingDiff(double a, double b)
{
    return std::abs(headingDiff(a, b));
}

double reversedHeading(double heading)
{
    if (heading >= 180.0)
        return heading - 180.0;
    else
        return heading + 180.0;
}

LineStringProjectionResult projectOntoLineString(Point const & point, LineString const & lineString)
{
    assert(lineString.size() > 1);

    auto minSnapped = snap(point, Segment{lineString[0], lineString[1]});
    auto minDistance = geoDistance(point, minSnapped.first);
    size_t idx = 1;
    for (size_t i = 2; i < lineString.size(); ++i)
    {
        auto snapped = snap(point, Segment{lineString[i - 1u], lineString[i]});
        auto distance = geoDistance(point, snapped.first);

        if (distance < minDistance)
        {
            minSnapped = snapped;
            minDistance = distance;
            idx = i;
        }
    }

    auto result = LineStringProjectionResult{};
    result.projectionPoint = minSnapped.first;
    result.distanceToSegment = minDistance;
    result.segmentHeading = heading(Segment{lineString[idx - 1u], lineString[idx]});

    result.distanceAlongLineString = 0.0;
    for (size_t i = 1; i < idx; ++i)
    {
        result.distanceAlongLineString += geoDistance(lineString[i - 1u], lineString[i]);
    }
    result.distanceAlongLineString += geoDistance(lineString[idx - 1u], minSnapped.first);

    if (idx == 1 and minSnapped.second < 0.0)
    {
        result.offLineString = true;
    }
    else if (idx == lineString.size() - 1u and minSnapped.second > 1.0)
    {
        result.offLineString = true;
    }
    else
    {
        result.offLineString = false;
    }
    return result;
}

double relativeDistanceAlongLineString(LineString const & lineString, Point const & point)
{
    auto result = projectOntoLineString(point, lineString);
    return result.distanceAlongLineString / geoLength(lineString);
}

/**
 * Returns the angle between segment1 and segment2 in range [180°, -180°].
 */
double angleBetweenSegments(Segment const & segment1, Segment const & segment2)
{
    auto headingSeg1 = heading(segment1);
    auto headingSeg2 = heading(segment2);
    return headingDiff(headingSeg1, headingSeg2);
}

std::vector<Core::Common::Geometry::Point> trimmed(std::vector<Core::Common::Geometry::Point> const & points, double const trimLeft, double const trimRight)
{
    std::vector<Core::Common::Geometry::Point> newPoints;
    std::shared_ptr<Core::Common::Geometry::Point> newStart;
    std::shared_ptr<Core::Common::Geometry::Point> newEnd;
    size_t oldBegin = 0;
    size_t oldEnd = points.size();

    double a = 0.0;
    for (size_t i = 0; i < points.size() - 1; ++i)
    {
        a += Core::Common::Geometry::geoDistance(points[i], points[i + 1]);
        if (a >= trimLeft)
        {
            oldBegin = i + 1;
            if (a > trimLeft)
                newStart = std::make_shared<Core::Common::Geometry::Point>(
                    Core::Common::Geometry::reverseHaversine(points[i + 1], Core::Common::Geometry::heading(points[i + 1], points[i]), a - trimLeft));
            break;
        }
    }

    a = 0.0;
    for (size_t i = points.size() - 1; i > 0; --i)
    {
        a += Core::Common::Geometry::geoDistance(points[i], points[i - 1]);
        if (a >= trimRight)
        {
            oldEnd = i;
            if (a > trimRight)
            {
                newEnd = std::make_shared<Core::Common::Geometry::Point>(
                    Core::Common::Geometry::reverseHaversine(points[i - 1], Core::Common::Geometry::heading(points[i - 1], points[i]), a - trimRight));
            }
            break;
        }
    }

    if (newStart)
        newPoints.push_back(*newStart);
    newPoints.insert(newPoints.end(), points.begin() + oldBegin, points.begin() + oldEnd);
    if (newEnd)
        newPoints.push_back(*newEnd);

    return newPoints;
}

}  // namespace Core::Common::Geometry


/**
 * @file local_coordinate_coding_test.cpp
 * @author Nishant Mehta
 *
 * Test for Local Coordinate Coding.
 *
 * mlpack is free software; you may redistribute it and/or modify it under the
 * terms of the 3-clause BSD license.  You should have received a copy of the
 * 3-clause BSD license along with mlpack.  If not, see
 * http://www.opensource.org/licenses/BSD-3-Clause for more information.
 */

// Note: We don't use BOOST_REQUIRE_CLOSE in the code below because we need
// to use FPC_WEAK, and it's not at all intuitive how to do that.
#include <mlpack/methods/local_coordinate_coding/lcc.hpp>

#include <boost/test/unit_test.hpp>
#include "test_tools.hpp"
#include "serialization.hpp"

using namespace arma;
using namespace mlpack;
using namespace mlpack::regression;
using namespace mlpack::lcc;

BOOST_AUTO_TEST_SUITE(LocalCoordinateCodingTest);

void VerifyCorrectness(const vec& beta, const vec& errCorr, double lambda)
{
  const double tol = 0.1;
  size_t nDims = beta.n_elem;
  for (size_t j = 0; j < nDims; j++)
  {
    if (beta(j) == 0)
    {
      // make sure that errCorr(j) <= lambda
      BOOST_REQUIRE_SMALL(std::max(fabs(errCorr(j)) - lambda, 0.0), tol);
    }
    else if (beta(j) < 0)
    {
      // make sure that errCorr(j) == lambda
      BOOST_REQUIRE_SMALL(errCorr(j) - lambda, tol);
    }
    else
    { // beta(j) > 0
      // make sure that errCorr(j) == -lambda
      BOOST_REQUIRE_SMALL(errCorr(j) + lambda, tol);
    }
  }
}

BOOST_AUTO_TEST_CASE(LocalCoordinateCodingTestCodingStep)
{
  double lambda1 = 0.1;
  uword nAtoms = 10;

  mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");
  uword nPoints = X.n_cols;

  // normalize each point since these are images
  for (uword i = 0; i < nPoints; i++)
  {
    X.col(i) /= norm(X.col(i), 2);
  }

  mat Z;
  LocalCoordinateCoding lcc(X, nAtoms, lambda1, 10);
  lcc.Encode(X, Z);

  mat D = lcc.Dictionary();

  for (uword i = 0; i < nPoints; i++)
  {
    vec sqDists = vec(nAtoms);
    for (uword j = 0; j < nAtoms; j++)
    {
      sqDists[j] = arma::norm(D.col(j) - X.col(i));
    }
    mat Dprime = D * diagmat(1.0 / sqDists);
    mat zPrime = Z.unsafe_col(i) % sqDists;

    vec errCorr = trans(Dprime) * (Dprime * zPrime - X.unsafe_col(i));
    VerifyCorrectness(zPrime, errCorr, 0.5 * lambda1);
  }
}

BOOST_AUTO_TEST_CASE(LocalCoordinateCodingTestDictionaryStep)
{
  const double tol = 0.1;

  double lambda = 0.1;
  uword nAtoms = 10;

  mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");
  uword nPoints = X.n_cols;

  // normalize each point since these are images
  for (uword i = 0; i < nPoints; i++)
  {
    X.col(i) /= norm(X.col(i), 2);
  }

  mat Z;
  LocalCoordinateCoding lcc(X, nAtoms, lambda, 10);
  lcc.Encode(X, Z);
  uvec adjacencies = find(Z);
  lcc.OptimizeDictionary(X, Z, adjacencies);

  mat D = lcc.Dictionary();

  mat grad = zeros(D.n_rows, D.n_cols);
  for (uword i = 0; i < nPoints; i++)
  {
    grad += (D - repmat(X.unsafe_col(i), 1, nAtoms)) *
        diagmat(abs(Z.unsafe_col(i)));
  }
  grad = lambda * grad + (D * Z - X) * trans(Z);

  BOOST_REQUIRE_SMALL(norm(grad, "fro"), tol);
}

BOOST_AUTO_TEST_CASE(SerializationTest)
{
  mat X = randu<mat>(100, 100);
  size_t nAtoms = 10;

  LocalCoordinateCoding lcc(nAtoms, 0.05, 2 /* don't care about quality */);
  lcc.Train(X);

  mat Y = randu<mat>(100, 200);
  mat codes;
  lcc.Encode(Y, codes);

  LocalCoordinateCoding lccXml(50, 0.1), lccText(12, 0.0), lccBinary(0, 0.0);
  SerializeObjectAll(lcc, lccXml, lccText, lccBinary);

  CheckMatrices(lcc.Dictionary(), lccXml.Dictionary(), lccText.Dictionary(),
      lccBinary.Dictionary());

  mat xmlCodes, textCodes, binaryCodes;
  lccXml.Encode(Y, xmlCodes);
  lccText.Encode(Y, textCodes);
  lccBinary.Encode(Y, binaryCodes);

  CheckMatrices(codes, xmlCodes, textCodes, binaryCodes);

  // Check the parameters, too.
  BOOST_REQUIRE_EQUAL(lcc.Atoms(), lccXml.Atoms());
  BOOST_REQUIRE_EQUAL(lcc.Atoms(), lccText.Atoms());
  BOOST_REQUIRE_EQUAL(lcc.Atoms(), lccBinary.Atoms());

  BOOST_REQUIRE_CLOSE(lcc.Tolerance(), lccXml.Tolerance(), 1e-5);
  BOOST_REQUIRE_CLOSE(lcc.Tolerance(), lccText.Tolerance(), 1e-5);
  BOOST_REQUIRE_CLOSE(lcc.Tolerance(), lccBinary.Tolerance(), 1e-5);

  BOOST_REQUIRE_CLOSE(lcc.Lambda(), lccXml.Lambda(), 1e-5);
  BOOST_REQUIRE_CLOSE(lcc.Lambda(), lccText.Lambda(), 1e-5);
  BOOST_REQUIRE_CLOSE(lcc.Lambda(), lccBinary.Lambda(), 1e-5);

  BOOST_REQUIRE_EQUAL(lcc.MaxIterations(), lccXml.MaxIterations());
  BOOST_REQUIRE_EQUAL(lcc.MaxIterations(), lccText.MaxIterations());
  BOOST_REQUIRE_EQUAL(lcc.MaxIterations(), lccBinary.MaxIterations());
}

/**
 * Test that LocalCoordinateCoding::Train() returns finite final objective
 * value.
 */
BOOST_AUTO_TEST_CASE(LocalCoordinateCodingTrainReturnObjective)
{
  double lambda1 = 0.1;
  uword nAtoms = 10;

  mat X;
  X.load("mnist_first250_training_4s_and_9s.arm");
  uword nPoints = X.n_cols;

  // Normalize each point since these are images.
  for (uword i = 0; i < nPoints; i++)
  {
    X.col(i) /= norm(X.col(i), 2);
  }

  LocalCoordinateCoding lcc(nAtoms, lambda1, 10);
  double objVal = lcc.Train(X);

  BOOST_REQUIRE_EQUAL(std::isfinite(objVal), true);
}

BOOST_AUTO_TEST_SUITE_END();


#include "model_distribution.hpp"

#include "monte_carlo_integration.hpp"
#include "multinomial.hpp"
#include "multivariate_guassian.hpp"
#include "mvi3/mvi3.hpp"
#include "types.hpp"
#include "utils.hpp"

#include <algorithm>
#include <boost/log/trivial.hpp>
#include <numeric>
#include <vector>

namespace FilterModel {
ModelDistribution::ModelDistribution(const std::vector<category_counts_t> &data,
                                     std::default_random_engine &generator, const Options &options)
    : data(data), generator(generator), options(options){};

std::vector<std::vector<double>> ModelDistribution::distribution(
    const std::vector<std::vector<alpha_t>> &alphas_per_object, double epsilon,
    const delta_t &delta) const {
    // See function definition for description.
    std::vector<std::vector<double>> log_alpha_likelyhoods_per_object(data.size(),
                                                                      std::vector<double>());
    for (int obs_index = 0; obs_index < data.size(); ++obs_index) {
        const category_counts_t &object_counts = data.at(obs_index);
        const std::vector<alpha_t> &alphas = alphas_per_object.at(obs_index);

        int n = std::accumulate(object_counts.begin(), object_counts.end(), 0);
        std::vector<double> p = {1 - epsilon, epsilon};
        Multinomial n_positive_distribution(n, p);

        for (int alpha_index = 0; alpha_index < alphas.size(); ++alpha_index) {
            const alpha_t &alpha = alphas.at(alpha_index);

            // n^+ goes from 0 to the sum of k_i for which alpha_i is true
            // When alpha_i is false, then all of the k_i corresponding must be noise reads.
            const category_counts_t filtered_object_counts = filter_by_alpha(alpha, object_counts);
            const int max_n_positive =
                std::accumulate(filtered_object_counts.begin(), filtered_object_counts.end(), 0);

            std::vector<double> log_p_k_n_positive_given_all_terms;
            // n_positive is the number of observations not made by the error process.
            for (int n_positive = 0; n_positive <= max_n_positive; ++n_positive) {
                // n_negative is the number of observations made by the error process.
                int n_negative = n - n_positive;
                double log_p_n_plus_given_n_epsilon =
                    n_positive_distribution.log_pdf(std::vector<int>({n_positive, n_negative}));

                double log_p_k_positive_given_alpha_n_positive =
                    calculate_log_p_k_positive_given_alpha_n_positive(n_positive, alpha);

                double log_sum_over_k_negative = calculate_log_sum_over_k_negative(
                    n_positive, n_negative, alpha, delta, object_counts);

                double log_p_k_n_positive_given_all = log_p_n_plus_given_n_epsilon +
                                                      log_p_k_positive_given_alpha_n_positive +
                                                      log_sum_over_k_negative;

                log_p_k_n_positive_given_all_terms.push_back(log_p_k_n_positive_given_all);
            }

            std::vector<double> p_k_n_positive_given_all =
                exp<double>(log_p_k_n_positive_given_all_terms);

            log_alpha_likelyhoods_per_object.at(obs_index).push_back(
                std::log(stable_sum<double>(p_k_n_positive_given_all)));
        }
    }
    return log_alpha_likelyhoods_per_object;
}

std::vector<std::vector<double>> ModelDistribution::distribution(const std::vector<alpha_t> &alphas,
                                                                 double epsilon,
                                                                 const delta_t &delta) const {
    std::vector<std::vector<alpha_t>> alphas_per_obs(data.size(), alphas);
    return distribution(alphas_per_obs, epsilon, delta);
}

double ModelDistribution::calculate_log_p_k_positive_given_alpha_n_positive(int n_positive,
                                                                            const alpha_t &alpha) {
    int sum_alpha = accumulate(alpha.begin(), alpha.end(), 0);

    // 1/(n^+ + sum alpha_i - 1 choose n^+)
    // lgamma(x + 1) = log(x!)
    double log_p_k_given_alpha_n_positive =
        lgamma(n_positive + 1) + lgamma(sum_alpha) - lgamma(n_positive + sum_alpha);
    return log_p_k_given_alpha_n_positive;
}

double ModelDistribution::calculate_log_sum_over_k_negative(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) const {
    // TODO(joschnie): Check for viability and return -1 if unviable.

    int n = n_positive + n_negative;

    delta_t delta_for_alpha_true = filter_by_alpha(alpha, delta);

    double sum_over_k_negative;
    if (options.comparison) {
        sum_over_k_negative = calculate_sum_over_k_negative_exact(n_positive, n_negative, alpha,
                                                                  delta, object_counts);
        double sum_over_k_negative_approx = calculate_sum_over_k_negative_approx(
            n_positive, n_negative, alpha, delta, object_counts);
        double error = sum_over_k_negative - sum_over_k_negative_approx;

        BOOST_LOG_TRIVIAL(info) << "Approximation error: " << std::to_string(error);
    } else {
        if (!can_use_normal_approx(n_negative, delta_for_alpha_true)) {
            sum_over_k_negative = calculate_sum_over_k_negative_exact(n_positive, n_negative, alpha,
                                                                      delta, object_counts);
        } else {
            sum_over_k_negative = calculate_sum_over_k_negative_approx(n_positive, n_negative,
                                                                       alpha, delta, object_counts);
        }
    }

    return std::log(sum_over_k_negative);
}

double ModelDistribution::calculate_sum_over_k_negative_exact(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) {
    Multinomial k_negative_distribution(n_negative, delta);

    std::vector<double> log_p_k_positive_given_delta_n_negative;

    iterate_over_k_negatives(
        [&log_p_k_positive_given_delta_n_negative, &k_negative_distribution](
            int k_1_negative, int k_2_negative, int k_3_negative) {
            log_p_k_positive_given_delta_n_negative.push_back(
                k_negative_distribution.log_pdf({k_1_negative, k_2_negative, k_3_negative}));
        },
        n_positive, n_negative, alpha, object_counts);

    std::vector<double> p_k_positive_given_delta_n_negative =
        exp(log_p_k_positive_given_delta_n_negative);
    return stable_sum<double>(p_k_positive_given_delta_n_negative);
}

void ModelDistribution::iterate_over_k_negatives(std::function<void(int, int, int)> f,
                                                 int n_positive, int n_negative,
                                                 const alpha_t &alpha,
                                                 const category_counts_t &object_counts) {
    // Because some alpha_i might be 0, we have to fix some of the k^- values to start with.
    std::vector<int> fixed_k_negative = {0, 0, 0};
    for (int i = 0; i < 3; ++i) {
        if (alpha.at(i) == false) {
            fixed_k_negative.at(i) = object_counts.at(i);
        }
    }

    int k_1_negative_start = std::max({object_counts[0] - n_positive, fixed_k_negative[0],
                                       n_negative - object_counts[1] - object_counts[2]});
    int k_1_negative_end =
        std::min(n_negative - fixed_k_negative[1] - fixed_k_negative[2], object_counts[0]);
    for (int k_1_negative = k_1_negative_start; k_1_negative <= k_1_negative_end; ++k_1_negative) {
        int k_2_negative_start = std::max({object_counts[1] - n_positive, fixed_k_negative[1],
                                           n_negative - k_1_negative - object_counts[2]});
        int k_2_negative_end =
            std::min(n_negative - k_1_negative - fixed_k_negative[2], object_counts[1]);
        for (int k_2_negative = k_2_negative_start; k_2_negative <= k_2_negative_end;
             ++k_2_negative) {
            int k_3_negative = n_negative - k_1_negative - k_2_negative;
            if (alpha[2] == false && k_3_negative == object_counts[2] ||
                alpha[2] == true && k_3_negative >= 0 && k_3_negative <= object_counts[2]) {
                f(k_1_negative, k_2_negative, k_3_negative);
            }
        }
    }
}

double ModelDistribution::calculate_sum_over_k_negative_approx(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) {
    std::vector<bool> object_count_nonzero(object_counts.size(), false);
    std::transform(object_counts.begin(), object_counts.end(), object_count_nonzero.begin(),
                   [](int count) { return count > 0; });

    std::vector<bool> effective_alpha;
    for (int i = 0; i < alpha.size(); ++i) {
        effective_alpha.push_back(alpha.at(i) && object_count_nonzero.at(i));
    }

    category_counts_t filtered_object_counts = filter_by_alpha(effective_alpha, object_counts);

    if (effective_alpha == std::vector<bool>({0, 0, 0})) {
        return 0;
    }

    Multinomial m_fixed =
        Multinomial(n_negative, delta)
            .fix_dimensions(not_v(effective_alpha),
                            filter_by_alpha(effective_alpha, object_counts, false));

    double sum_over_k_negative_approx;
    if (m_fixed.p.size() > 1) {
        MultivariateGuassian mg = MultivariateGuassian::from_multinomial(m_fixed);
        std::vector<std::vector<double>> hyperplanes =
            ModelDistribution::get_hyperplanes(m_fixed.n, filtered_object_counts);
        mg.shift_hyperplanes(hyperplanes);

        MVI3::Mvi3 mvi3;
        double log_sum_over_k_negative_approx =
            std::log(mvi3.integrate(12456, -1, 10, 10, mg.get_covariance(), hyperplanes)) +
            m_fixed.log_adjust;

        sum_over_k_negative_approx = std::exp(log_sum_over_k_negative_approx);
    } else {
        if (filtered_object_counts.size() > 0 && filtered_object_counts.back() >= m_fixed.n) {
            sum_over_k_negative_approx = std::exp(m_fixed.log_adjust);
            // TODO(joschnei): condition above is wrong. Need to check both n+ and n-b
        } else {
            sum_over_k_negative_approx = 0;
        }
    }
    return sum_over_k_negative_approx;
}

double ModelDistribution::calculate_sum_over_k_negative_approx_2(
    int n_positive, int n_negative, const alpha_t &alpha, const delta_t &delta,
    const category_counts_t &object_counts) {
    Multinomial m_fixed =
        Multinomial(n_negative, delta)
            .fix_dimensions(not_v(alpha), filter_by_alpha(alpha, object_counts, false));
    MultivariateGuassian mg = MultivariateGuassian::from_multinomial(m_fixed);

    std::function<double(std::vector<double>)> multivariate_guassian_density =
        [mg](std::vector<double> point) { return mg.density(point); };

    std::function<std::vector<double>(std::default_random_engine &)> sampler =
        [m_fixed, object_counts = filter_by_alpha(alpha, object_counts)](
            std::default_random_engine &generator) {
            int dim = m_fixed.p.size();

            std::vector<std::uniform_real_distribution<double>> dists;
            for (int i = 0; i < dim - 1; ++i) {
                dists.push_back(std::uniform_real_distribution<double>(0.0, object_counts.at(i)));
            }

            std::vector<double> point;
            double last_component;
            do {
                point = std::vector<double>();
                for (std::uniform_real_distribution<double> dist : dists) {
                    point.push_back(dist(generator));
                }
                last_component = m_fixed.n - std::accumulate(point.begin(), point.end(), 0.0);
            } while (!(0 <= last_component && last_component <= object_counts.back()));
            return point;
        };

    double volume;
    if (m_fixed.p.size() == 3) {
        volume = n_negative * object_counts.at(2) - std::pow(object_counts.at(2), 2) / 2.0;
        if (n_negative > object_counts.at(0)) {
            volume -= 1.0 / 2.0 * std::pow(n_negative - object_counts.at(0), 2);
            if (n_negative - object_counts.at(2) > object_counts.at(0)) {
                volume +=
                    1.0 / 2.0 * std::pow(n_negative - object_counts.at(2) - object_counts.at(0), 2);
            }
        }
        if (n_negative > object_counts.at(1)) {
            volume -= 1.0 / 2.0 * std::pow(n_negative - object_counts.at(1), 2);
            if (n_negative - object_counts.at(2) > object_counts.at(1)) {
                volume +=
                    1.0 / 2.0 * std::pow(n_negative - object_counts.at(2) - object_counts.at(1), 2);
            }
        }
        if (n_negative > object_counts.at(0) + object_counts.at(1)) {
            volume +=
                1.0 / 2.0 * std::pow(n_negative - object_counts.at(0) - object_counts.at(1), 2);
        }
        if (n_negative - object_counts.at(2) < 0) {
            volume += 1.0 / 2.0 * std::pow(n_negative - object_counts.at(2), 2.0);
        }
    } else if (m_fixed.p.size() == 2) {
        volume = std::min(n_negative, object_counts.at(0)) -
                 std::max(0, n_negative - object_counts.at(1));
    }

    return std::exp(
        std::log(integrate(multivariate_guassian_density, sampler, generator, volume, 500000)) +
        m_fixed.log_adjust);
}

/**
 * Assumes n_negative has already been adjusted, objct_counts has already been filtered.
 */
std::vector<std::vector<double>> ModelDistribution::get_hyperplanes(
    int n_negative, category_counts_t object_counts) {
    std::vector<std::vector<double>> hyperplanes;

    // Of the the remaining dimensions, pick the last one to equal n - sum k_i.
    category_count_t k_fixed = object_counts.back();
    object_counts.pop_back();

    // sum k_i^- = n^- => k_1^- = n^- - sum k_i^- (from 2)
    // k_1^- >= 0 => sum k_i^- (from 2) <= n^-
    std::vector<double> k_fixed_negative_lower_constraint(object_counts.size(), 1);
    k_fixed_negative_lower_constraint.push_back(n_negative);
    hyperplanes.push_back(k_fixed_negative_lower_constraint);

    // sum k_i^- = n^+- => k_1^- = n^- - sum k_i^- (from 2)
    // k_1^- <= k_1 => - sum k_i^- (from 2) <= k_1 - n^-
    std::vector<double> k_fixed_negative_upper_constraint(object_counts.size(), -1);
    k_fixed_negative_upper_constraint.push_back(k_fixed - n_negative);
    hyperplanes.push_back(k_fixed_negative_upper_constraint);

    // k_i^+ and k_i^- are both >= 0.
    // k_i^- >= 0 implies -k_i^- <= 0.
    // k_i+ >= 0 implies k_i - k_i^- >= 0 implies k_i^- <= k_i
    for (int i = 0; i < object_counts.size(); ++i) {
        std::vector<double> k_i_minus_constraint(object_counts.size(), 0);
        k_i_minus_constraint.at(i) = -1;
        k_i_minus_constraint.push_back(0);
        hyperplanes.push_back(k_i_minus_constraint);

        std::vector<double> k_i_plus_constraint(object_counts.size(), 0);
        k_i_plus_constraint.at(i) = 1;
        k_i_plus_constraint.push_back(object_counts.at(i));
        hyperplanes.push_back(k_i_plus_constraint);
    }

    return hyperplanes;
}

bool ModelDistribution::can_use_normal_approx(int n_negative, const delta_t &delta) const {
    if (options.exact) {
        return false;
    }
    for (double p : delta) {
        if (n_negative * p <= 5.0) {
            return false;
        }
    }
    return true;
}
}  // namespace FilterModel


// Std includes
#include <iostream> // cout, endl
#include <vector>
#include <memory> // shared_ptr
#include <map>
// Thirdparties includes
#include <Eigen/Dense>
// Lib includes
#include "v0l/bin/file_data.h"
#include "m0sh/uniform.h"
#include "m0sh/structured_sub.h"
#include "fl0p/stationary.h"

const unsigned int DIM = 3;

using TypeScalar = double;
using TypeVector = Eigen::Matrix<TypeScalar, DIM, 1>;
using TypeMatrix = Eigen::Matrix<TypeScalar, DIM, DIM>;
template<typename... Args>
using TypeRef = Eigen::Ref<Args...>;

template<typename ...Args>
using TypeContainer = std::vector<Args...>;
using TypeMesh = m0sh::Uniform<TypeVector, TypeRef, TypeContainer>;
using TypeMeshSub = m0sh::StructuredSub<TypeVector, TypeRef, TypeContainer>;
using TypeFlow = fl0w::fl0p::Stationary<TypeVector, TypeMatrix, TypeRef, TypeMesh, TypeContainer, TypeMeshSub, v0l::FileData>;

void print(const TypeFlow& flow, const TypeVector& x, const TypeScalar& t) {
    std::cout << std::endl;
    std::cout << "flow.getVelocity(" << x.transpose() << ", " << t << ") = \n" << flow.getVelocity(x, t).transpose() << std::endl;
    std::cout << std::endl;
}

int main () {
    v0l::FileData<float> vx("../data/v.vtk", 0);
    // mesh data
    // // create lengths
    TypeVector origin;
    std::vector<double> lengths;
    for(std::size_t i = 0; i < vx.meta.dimensions.size(); i++) {
        lengths.push_back(vx.meta.dimensions[i] * vx.meta.spacing[i]);
        origin[i] = vx.meta.origin[i];
    }
    // // other scalars
    v0l::FileData<float> vy("../data/v.vtk", 1);
    v0l::FileData<float> vz("../data/v.vtk", 2);
    // // flow
    std::cout << "building flow..." << std::endl;
    TypeFlow flow(std::make_shared<TypeMesh>(vx.meta.dimensions, lengths, origin, std::vector<bool>(DIM, true)), std::vector<v0l::FileData<float>>({vx, vy, vz}), 1);
    std::cout << "flow built !" << std::endl;

    // Print
    print(flow, TypeVector({-0.5, -0.5, -0.5}), 0.0);
    print(flow, TypeVector({0.0, 0.0, 0.0}), 0.0);
    print(flow, TypeVector({0.5, 0.5, 0.5}), 0.0);
}


#include <octomap/octomap.h>
#include <pcl/common/centroid.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>

#include <Eigen/Dense>
#include <unordered_map>
#include <unordered_set>

#define resolution 0.25  // unit: m

int main(int argc, char** argv) {
  std::string octomap_name = "octomap.ot";

  // Part1: Read origin point cloud
  Eigen::Vector4f centroid;
  Eigen::Matrix3f covariance_matrix;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(
      new pcl::PointCloud<pcl::PointXYZ>);

  std::cout << " --------------- "
            << " initial         "
            << " --------------- " << std::endl;

  if (argc == 2) {
    std::cout << "use input data" << std::endl;

    if (pcl::io::loadPCDFile(argv[1], *cloud_in)) {
      std::cerr << "failed to open " << argv[1] << std::endl;
      return 1;
    }

    pcl::computeMeanAndCovarianceMatrix(*cloud_in, covariance_matrix, centroid);

    Eigen::Matrix4f transform = Eigen::Matrix4f::Identity();
    transform(0, 3) = -centroid(0);
    transform(1, 3) = -centroid(1);
    transform(2, 3) = -centroid(2);

    pcl::transformPointCloud(*cloud_in, *cloud_in, transform);
  }

  std::cout << "Input " << cloud_in->size() << " pts. " << std::endl;

  // Part2: Construct Gaussion Octomap
  std::cout << " --------------- "
            << " compute tree    "
            << " --------------- " << std::endl;

  octomap::OcTree save_tree(resolution);
  std::unordered_multimap<octomap::OcTreeKey, pcl::PointXYZ,
                          octomap::OcTreeKey::KeyHash>
      unorderedMultiMap;
  std::unordered_set<octomap::OcTreeKey, octomap::OcTreeKey::KeyHash> set;

  for (auto p : (*cloud_in).points) {
    auto key = save_tree.coordToKey(p.x, p.y, p.z, 16);
    unorderedMultiMap.emplace(key, p);
    set.emplace(key);
  }

  for (auto iter = set.begin(); iter != set.end(); ++iter) {
    auto key = *iter;
    auto range = unorderedMultiMap.equal_range(key);
    int i = 0;

    pcl::PointCloud<pcl::PointXYZ> cloud;
    for (auto it = range.first; it != range.second; ++it) {
      cloud.push_back(it->second);
      i++;
    }

    if (cloud.size() < 2) continue;

    save_tree.updateNode(key, true);
  }

  save_tree.updateInnerOccupancy();

  // Part3: Octomap
  save_tree.write(octomap_name);
}


#include <cmath>

#include <string>
#include <iostream>
#include <random>
#include <algorithm>

#include <boost/math/constants/constants.hpp>

#define EIGEN_INITIALIZE_MATRICES_BY_NAN

#include <cmath>
#include <cstdlib>
#include <complex>
#include <type_traits>

#include <iostream>
#include <iomanip>

// define the exception class, but don't override eigen's eigen_assert() macro itself
#include <tomographer/tools/eigen_assert_exception.h>

#include <Eigen/Core>

#define BOOST_TEST_MODULE test_diamond_norm
#include <boost/test/unit_test.hpp>

#include "testutils.h"

#include "diamond_norm_figofmerit.h"
#include "diamond_norm_sdpa.h"
#include "diamond_norm_scs.h"

#include <tomographer/tools/boost_test_logger.h>
#include <tomographer/mathtools/pos_semidef_util.h>
#include <tomographer/densedm/dmtypes.h>

// -----------------------------------------------------------------------------
// fixture(s)

template<int QuDim_>
struct identdefs
{
  static constexpr int QuDim = QuDim_;
  static constexpr int QuDim2 = QuDim*QuDim;

  typedef Eigen::Matrix<std::complex<double>,QuDim2,1> PureKetType;
  typedef Eigen::Matrix<std::complex<double>,QuDim2,QuDim2> ChoiMatrixType;
  typedef Eigen::Matrix<std::complex<double>,QuDim,QuDim> ReducedDMType;

  PureKetType Psi;
  ChoiMatrixType  Eident;
  ChoiMatrixType  Eclassident;

  identdefs()
    : Psi(PureKetType::Zero())
  {
    BOOST_TEST_MESSAGE("identdefs(), QuDim="<<QuDim<<", QuDim2="<<QuDim2);

    // initialize Psi
    for (std::size_t j = 0; j < QuDim; ++j) {
      Psi(j+QuDim*j) = 1;
    }

    // initialize Eident
    Eident = Psi * Psi.adjoint();

    validate_channel<double>(Eident, QuDim, QuDim, "Eident");

    // initialize Eclassident (classical identity channel == fully depolarizing)
    Eclassident = ChoiMatrixType::Zero();
    for (std::size_t j = 0; j < QuDim; ++j) {
      Eclassident(QuDim*j+j, QuDim*j+j) = 1;
    }

    validate_channel<double>(Eclassident, QuDim, QuDim, "Eclassident");

    BOOST_TEST_MESSAGE("identdefs() done");
  }
};
template<int QuDim_> constexpr int identdefs<QuDim_>::QuDim;
template<int QuDim_> constexpr int identdefs<QuDim_>::QuDim2;

// -----------------------------------------------------------------------------
// test suites

BOOST_AUTO_TEST_SUITE(test_diamond_norm)
// =============================================================================

BOOST_AUTO_TEST_SUITE(examples)

BOOST_FIXTURE_TEST_CASE(simple_qubit_sdpa, identdefs<2>)
{
  // This is now a test channel which is hopefully close to Eclassident.
  ChoiMatrixType E;
  E <<
    0.9, 0, 0, 0,
    0, 0.1, 0, 0,
    0,   0, 0, 0,
    0,   0, 0, 1;

  const ChoiMatrixType Delta = E - Eclassident;

  Tomographer::Logger::BoostTestLogger lg(Tomographer::Logger::LONGDEBUG);

  DiamondNormSDPASolver<double,Tomographer::Logger::BoostTestLogger,true> d(2, 2, lg);
  double value = d.calculate(Delta);

  BOOST_CHECK_CLOSE( value, 0.1, 1e-1/*percent*/ );
}
BOOST_FIXTURE_TEST_CASE(simple_qubit_scs, identdefs<2>)
{
  // This is now a test channel which is hopefully close to Eclassident.
  ChoiMatrixType E;
  E <<
    0.9, 0, 0, 0,
    0, 0.1, 0, 0,
    0,   0, 0, 0,
    0,   0, 0, 1;

  const ChoiMatrixType Delta = E - Eclassident;

  Tomographer::Logger::BoostTestLogger lg(Tomographer::Logger::LONGDEBUG);

  DiamondNormSCSSolver<double,Tomographer::Logger::BoostTestLogger,true> d(2, 2, lg);
  double value = d.calculate(Delta);

  BOOST_CHECK_CLOSE( value, 0.1, 1e-1/*percent*/ );
}

BOOST_FIXTURE_TEST_CASE(qutrit_case_sdpa, identdefs<3>)
{
  // This is now a test channel which is hopefully close to Eident.

  ChoiMatrixType E;
  E  <<
    0.9,0,  0,  0,  0.5,0,  0,  0,0.2,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,0.1,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0.5,0,  0,  0,  1,  0,  0,  0,0.8,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
  0.2,  0,  0,  0,0.8,  0,  0,  0,  1 ;

  ChoiMatrixType Delta = E - Eident;

  BOOST_TEST_MESSAGE("\n\nAll good, continuing.\n"
                     << "Delta is = \n"
                     << Delta << "\n\n");

  Tomographer::Logger::BoostTestLogger lg(Tomographer::Logger::LONGDEBUG);

  DiamondNormSDPASolver<double,Tomographer::Logger::BoostTestLogger,true> d(3, 3, lg);
  double value = d.calculate(Delta);

  // calculated by solving the SDP with MATLAB/CVX
  BOOST_CHECK_CLOSE( value, 0.42666667, 1e-4/*percent*/ );
}
BOOST_FIXTURE_TEST_CASE(qutrit_case_scs, identdefs<3>)
{
  // This is now a test channel which is hopefully close to Eident.

  ChoiMatrixType E;
  E  <<
    0.9,0,  0,  0,  0.5,0,  0,  0,0.2,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,0.1,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0.5,0,  0,  0,  1,  0,  0,  0,0.8,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
    0,  0,  0,  0,  0,  0,  0,  0,  0,
  0.2,  0,  0,  0,0.8,  0,  0,  0,  1 ;

  ChoiMatrixType Delta = E - Eident;

  BOOST_TEST_MESSAGE("\n\nAll good, continuing.\n"
                     << "Delta is = \n"
                     << Delta << "\n\n");

  Tomographer::Logger::BoostTestLogger lg(Tomographer::Logger::LONGDEBUG);

  DiamondNormSCSSolver<double,Tomographer::Logger::BoostTestLogger,true> d(3, 3, lg);
  double value = d.calculate(Delta);

  // calculated by solving the SDP with MATLAB/CVX
  BOOST_CHECK_CLOSE( value, 0.42666667, 1e-1/*percent*/ );
}

BOOST_AUTO_TEST_SUITE_END(); // examples

BOOST_FIXTURE_TEST_SUITE(diamond_norm_from_bistate, identdefs<2>)

BOOST_AUTO_TEST_CASE(dsolver_sdpa)
{
  Eigen::MatrixXcd true_E(4,4);
  true_E <<
    0.95,  0.,    0.,    0.95,
    0.,    0.05,  0.,    0.,
    0.,    0.,    0.,    0.,
    0.95,  0.,    0.,    1. ;

  DiamondNormSDPASolver<double, Tomographer::Logger::VacuumLogger> dnslv(2, 2, Tomographer::Logger::vacuum_logger);

  double val = dnslv.calculate(true_E - Eident);

  BOOST_TEST_MESSAGE("diamond-norm(true_E) = " << val);
  // value calculated using Python/QuTip
  BOOST_CHECK_CLOSE(val, 0.05, 1e-1/*percent*/) ;
}
BOOST_AUTO_TEST_CASE(dsolver_scs)
{
  Eigen::MatrixXcd true_E(4,4);
  true_E <<
    0.95,  0.,    0.,    0.95,
    0.,    0.05,  0.,    0.,
    0.,    0.,    0.,    0.,
    0.95,  0.,    0.,    1. ;

  DiamondNormSCSSolver<double, Tomographer::Logger::VacuumLogger> dnslv(2, 2, Tomographer::Logger::vacuum_logger);

  double val = dnslv.calculate(true_E - Eident);

  BOOST_TEST_MESSAGE("diamond-norm(true_E) = " << val);
  // value calculated using Python/QuTip
  BOOST_CHECK_CLOSE(val, 0.05, 1e-1/*percent*/) ;
}

BOOST_AUTO_TEST_CASE(example)
{
  Eigen::MatrixXcd rho(4,4);
  rho <<
    5.69952010e-01,   5.22989693e-03,   5.22989693e-03,   4.65354072e-01,
    5.22989693e-03,   3.00479897e-02,   4.79896928e-05,   4.77010307e-03,
    5.22989693e-03,   4.79896928e-05,   4.79896928e-05,   4.27010307e-03,
    4.65354072e-01,   4.77010307e-03,   4.27010307e-03,   3.99952010e-01;

  BOOST_TEST_MESSAGE("rho = \n" << rho );

  Eigen::MatrixXcd T = Tomographer::MathTools::safeOperatorSqrt<Eigen::MatrixXcd>(rho);
  BOOST_TEST_MESSAGE("T = \n" << T);

  Tomographer::DenseDM::DMTypes<4> dmt(4);

  {
    DiamondNormToRefValueCalculator<Tomographer::DenseDM::DMTypes<4>,
                                    DiamondNormSDPASolver<double, Tomographer::Logger::VacuumLogger> > valcalc(dmt, Eident, 2);

    double val = valcalc.getValue(T);
    // value calculated using Python/QuTip
    BOOST_CHECK_CLOSE(val, 0.05, 1e-1/*percent*/) ;
  }
  {
    DiamondNormToRefValueCalculator<Tomographer::DenseDM::DMTypes<4>,
                                    DiamondNormSCSSolver<double, Tomographer::Logger::VacuumLogger> > valcalc(dmt, Eident, 2);

    double val = valcalc.getValue(T);
    // value calculated using Python/QuTip
    BOOST_CHECK_CLOSE(val, 0.05, 1e-1/*percent*/) ;
  }
}

BOOST_AUTO_TEST_SUITE_END();

// =============================================================================
BOOST_AUTO_TEST_SUITE_END() ;


/*!
* \file GrapheneExp.cpp
*
* \brief Tight-binding model for graphene photo-excitation
* Solves the ODE system for a wave with a few-cycle envelope (exp)
*
* The excitation
* is a slowly varying envelope with exponential shape
* See e.g. http://link.aps.org/doi/10.1103/PhysRevB.91.045439
*
* \author Author: D. Gagnon <denisg6@hotmail.com>
*/

// Include some headers
#include <iostream>
#include <fstream>
#include <armadillo>
#include <cmath>
#include <complex>

#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/ini_parser.hpp>
#include <boost/numeric/odeint.hpp>

#include "GrapheneExp.hpp"
#include "Utils.hpp"

/// Main function
int main(int argc, char *argv[])
{
    // Parse parameter file
    boost::property_tree::ptree pt;
    boost::property_tree::ini_parser::read_ini("GrapheneExp.ini", pt);

    // Problem parameters
    double E0 = std::stof(pt.get<std::string>("Parameters.E0"));            // Peak electric field in V/m
    double tau = std::stof(pt.get<std::string>("Parameters.tau"));        // Pulse duration in fs

    // Parameter sweep
    double xmin = std::stof(pt.get<std::string>("Sweep.xmin")); // Minimum frequency in sweep
    double xmax = std::stof(pt.get<std::string>("Sweep.xmax")); // Minimum frequency in sweep
    int x_elem = std::stoi(pt.get<std::string>("Sweep.xnum"));

    // Parameter sweep
    double ymin = std::stof(pt.get<std::string>("Sweep.ymin")); // Minimum frequency in sweep
    double ymax = std::stof(pt.get<std::string>("Sweep.ymax")); // Minimum frequency in sweep
    int y_elem = std::stoi(pt.get<std::string>("Sweep.ynum"));

    // Meshgrid (vectors of parameters)
    auto xvec = arma::linspace(xmin, xmax, x_elem);
    auto yvec = arma::linspace(ymin, ymax, y_elem);

    // Variables to store probability values
    auto prob_mat = arma::mat(y_elem,x_elem);

    // Initial value of time, integration time and interval

    double tinit = -4.0*tau;
    double inttime = 8.0*tau;
    double dt = inttime/100;

    // Variables for loops
    unsigned id, id2;

    // Loop for each K value and compute probability
    for (id=0; id < y_elem; id++)
    {
        for (id2=0; id2 < x_elem; id2++)
        {

            // Initialize tight-binding model
            tight_binding_exp model(xvec[id2],yvec[id],tau,E0);

            // Prepare initial states (negative and positive eigenstates)
            state_type psi = EigenState(model, xvec[id2], yvec[id], -1.0);
            state_type eigen_p = EigenState(model, xvec[id2], yvec[id], 1.0);

            // Integrate
            size_t steps = boost::numeric::odeint::integrate( model,
                           psi, tinit, inttime, dt );

            // Calculate the projection
            std::complex<double> projection = Projection(eigen_p,psi);

            prob_mat(id,id2) = std::abs(projection)*std::abs(projection);

        }

    }

    // Save ODE integration data
    prob_mat.save("probability.dat", arma::raw_ascii);

    // Save parameters vector
    xvec.save("xvec.dat", arma::raw_ascii);
    yvec.save("yvec.dat", arma::raw_ascii);

    // For post-processing: time evolution of vector potential after ODE integration
    int numtimes = ceil(inttime/dt);
    auto times = arma::linspace(tinit,tinit+inttime,numtimes);

    // Initialize output file
    std::ofstream outfile("potential.dat");
    outfile << std::scientific << std::setprecision(10);

    // Create tight binding object
    tight_binding_exp model(0.0,0.0,tau,E0);

    for (size_t i=0; i < numtimes; i++)
    {
        outfile << times(i) << " "
                << model.Gx(times(i)) << " "
                << model.Gy(times(i)) << std::endl;
    }

    return 0;

}


#include "catch.hpp"
#include "timer.hpp"

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <Eigen/SparseCholesky>
#include <iostream>
#include <vector>

using namespace Eigen;
using Td = Triplet<double>;

TEST_CASE("tridiagonal eye", "[inv]") {
  int sb = 4;
  int nb = 4;
  int ndim = nb * sb;
  SparseMatrix<double> matA(ndim, ndim);
  std::vector<Td> tripletList;
  tripletList.reserve(ndim * 3 - sb * 2);
  for (int i = 0; i < ndim; ++i) {
    tripletList.push_back(Td(i, i, 1.0));
  }
  for (int i = sb; i < ndim; ++i) {
    tripletList.push_back(Td(i - sb, i, 1.0));
    tripletList.push_back(Td(i, i - sb, 1.0));
  }
  matA.setFromTriplets(tripletList.begin(), tripletList.end());
  SparseMatrix<double> matI(ndim, ndim);
  matI.setIdentity();

  Timer::begin("eigen sparse solver");
  SimplicialLDLT<SparseMatrix<double>> solver;
  solver.compute(matA);
  MatrixXd A_inv = solver.solve(matI);
  Timer::end("eigen sparse solver");

  IOFormat fmt(1, 0, ", ", "\n", "[", "]");
  std::cout << A_inv.format(fmt) << std::endl;

  std::cout << Timer::summery() << std::endl;
}


//=======================================================================
// Copyright 2001 Jeremy G. Siek, Andrew Lumsdaine, Lie-Quan Lee,
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/kruskal_min_spanning_tree.hpp>
#include <iostream>
#include <fstream>

int
main()
{
  using namespace boost;
  typedef adjacency_list < vecS, vecS, undirectedS,
    no_property, property < edge_weight_t, int > > Graph;
  typedef graph_traits < Graph >::edge_descriptor Edge;
  typedef std::pair<int, int> E;

  const int num_nodes = 5;
  E edge_array[] = { E(0, 2), E(1, 3), E(1, 4), E(2, 1), E(2, 3),
    E(3, 4), E(4, 0), E(4, 1)
  };
  int weights[] = { 1, 1, 2, 7, 3, 1, 1, 1 };
  std::size_t num_edges = sizeof(edge_array) / sizeof(E);
#if defined(BOOST_MSVC) && BOOST_MSVC <= 1300
  Graph g(num_nodes);
  property_map<Graph, edge_weight_t>::type weightmap = get(edge_weight, g);
  for (std::size_t j = 0; j < num_edges; ++j) {
    Edge e; bool inserted;
    boost::tie(e, inserted) = add_edge(edge_array[j].first, edge_array[j].second, g);
    weightmap[e] = weights[j];
  }
#else
  Graph g(edge_array, edge_array + num_edges, weights, num_nodes);
#endif
  property_map < Graph, edge_weight_t >::type weight = get(edge_weight, g);
  std::vector < Edge > spanning_tree;

  kruskal_minimum_spanning_tree(g, std::back_inserter(spanning_tree));

  std::cout << "Print the edges in the MST:" << std::endl;
  for (std::vector < Edge >::iterator ei = spanning_tree.begin();
       ei != spanning_tree.end(); ++ei) {
    std::cout << source(*ei, g) << " <--> " << target(*ei, g)
      << " with weight of " << weight[*ei]
      << std::endl;
  }

  std::ofstream fout("figs/kruskal-eg.dot");
  fout << "graph A {\n"
    << " rankdir=LR\n"
    << " size=\"3,3\"\n"
    << " ratio=\"filled\"\n"
    << " edge[style=\"bold\"]\n" << " node[shape=\"circle\"]\n";
  graph_traits<Graph>::edge_iterator eiter, eiter_end;
  for (boost::tie(eiter, eiter_end) = edges(g); eiter != eiter_end; ++eiter) {
    fout << source(*eiter, g) << " -- " << target(*eiter, g);
    if (std::find(spanning_tree.begin(), spanning_tree.end(), *eiter)
        != spanning_tree.end())
      fout << "[color=\"black\", label=\"" << get(edge_weight, g, *eiter)
           << "\"];\n";
    else
      fout << "[color=\"gray\", label=\"" << get(edge_weight, g, *eiter)
           << "\"];\n";
  }
  fout << "}\n";
  return EXIT_SUCCESS;
}


#include <boost/random/piecewise_constant_distribution.hpp>


// Boost.Geometry
// Robustness Test

// Copyright (c) 2019-2021 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is subject to the Boost Software License,
// Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

#define BOOST_GEOMETRY_NO_ROBUSTNESS

#include <iostream>
#include <iomanip>
#include <fstream>
#include <sstream>
#include <string>

#include <geometry_test_common.hpp>

#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>

// Basic case. Union should deliver 22.0
static std::string case_a[2] =
    {
    "MULTIPOLYGON(((0 0,0 4,2 4,2 3,4 3,4 0,0 0)))",
    "MULTIPOLYGON(((2 7,4 7,4 3,2 3,2 7)))"
    };

// Case with an interior ring. Union should deliver 73.0
static std::string case_b[2] =
    {
    "MULTIPOLYGON(((0 0,0 4,2 4,2 3,4 3,4 0,0 0)))",
    "MULTIPOLYGON(((-1 -1,-1 8,8 8,8 -1,-1 -1),(2 7,2 3,4 3,4 7,2 7)))"
    };

static std::string case_c[2] =
    {
    "MULTIPOLYGON(((0 0,0 4,2 4,2 3,4 3,4 0,0 0)))",
    "MULTIPOLYGON(((1 1,0 1,0 3,1 3,1 1)))"
    };

struct test_settings
{
    bool verbose{false};
    bool do_output{false};

    // Settings currently not modifiable, and still giving quite some errors
    double start_bound{1.0e-2};
    double step_factor{50.0};  // on each side -> 100 steps per factor
    int max_factor{10000};
};

template <bg::overlay_type OverlayType, typename Geometry>
bool test_overlay(std::string const& caseid,
        Geometry const& g1, Geometry const& g2,
        double expected_area,
        test_settings const& settings)
{

    typedef typename boost::range_value<Geometry>::type geometry_out;
    typedef bg::detail::overlay::overlay
        <
            Geometry, Geometry,
            bg::detail::overlay::do_reverse<bg::point_order<Geometry>::value>::value,
            OverlayType == bg::overlay_difference
            ? ! bg::detail::overlay::do_reverse<bg::point_order<Geometry>::value>::value
            : bg::detail::overlay::do_reverse<bg::point_order<Geometry>::value>::value,
            bg::detail::overlay::do_reverse<bg::point_order<Geometry>::value>::value,
            geometry_out,
            OverlayType
        > overlay;

    typedef typename bg::strategies::relate::services::default_strategy
        <
            Geometry, Geometry
        >::type strategy_type;

    strategy_type strategy;

    typedef typename bg::rescale_overlay_policy_type
    <
        Geometry,
        Geometry
    >::type rescale_policy_type;

    rescale_policy_type robust_policy
        = bg::get_rescale_policy<rescale_policy_type>(g1, g2);

    Geometry result;
    bg::detail::overlay::overlay_null_visitor visitor;
    overlay::apply(g1, g2, robust_policy, std::back_inserter(result),
                   strategy, visitor);

    const double detected_area = bg::area(result);
    if (std::fabs(detected_area - expected_area) > 0.01)
    {
        if (settings.do_output)
        {
            std::cout << "ERROR: " << caseid << std::setprecision(18)
                      << " detected=" << detected_area
                      << " expected=" << expected_area << std::endl
                      << "    " <<  bg::wkt(g1) << std::endl
                      << "    " << bg::wkt(g2) << std::endl;
        }
        return false;
    }
    return true;
}

template <typename Ring>
void update(Ring& ring, double x, double y, std::size_t index)
{
    if (index >= ring.size())
    {
        return;
    }
    bg::set<0>(ring[index], bg::get<0>(ring[index]) + x);
    bg::set<1>(ring[index], bg::get<1>(ring[index]) + y);
    if (index == 0)
    {
        ring.back() = ring.front();
    }
}

template <bg::overlay_type OverlayType, typename MultiPolygon>
std::size_t test_case(std::size_t& error_count,
        std::size_t case_index, std::size_t i, std::size_t j,
        std::size_t min_vertex_index, std::size_t max_vertex_index,
        double offset_x, double offset_y, double expectation,
        MultiPolygon const& poly1, MultiPolygon const& poly2,
        test_settings const settings)
{
    std::size_t n = 0;
    for (std::size_t k = min_vertex_index; k <= max_vertex_index; k++, ++n)
    {
        MultiPolygon poly2_adapted = poly2;

        switch (case_index)
        {
            case 2 :
                update(bg::interior_rings(poly2_adapted.front()).front(), offset_x, offset_y, k);
                break;
            default :
                update(bg::exterior_ring(poly2_adapted.front()), offset_x, offset_y, k);
                break;
        }

        std::ostringstream out;
        out << "case_" << i << "_" << j << "_" << k;
        if (! test_overlay<OverlayType>(out.str(), poly1, poly2_adapted, expectation, settings))
        {
            if (error_count == 0 && ! settings.do_output)
            {
                // First failure is always reported
                test_settings adapted = settings;
                adapted.do_output = true;
                test_overlay<OverlayType>(out.str(), poly1, poly2_adapted, expectation, adapted);
            }
            error_count++;
        }
    }
    return n;
}

template <typename T, bool Clockwise, bg::overlay_type OverlayType>
std::size_t test_all(std::size_t case_index, std::size_t min_vertex_index,
                     std::size_t max_vertex_index,
                     double expectation, test_settings const& settings)
{
    typedef bg::model::point<T, 2, bg::cs::cartesian> point_type;
    typedef bg::model::polygon<point_type, Clockwise> polygon;
    typedef bg::model::multi_polygon<polygon> multi_polygon;

    const std::string& first = case_a[0];

    const std::string& second
            = case_index == 1 ? case_a[1]
            : case_index == 2 ? case_b[1]
            : case_index == 3 ? case_c[1]
            : "";

    multi_polygon poly1;
    bg::read_wkt(first, poly1);

    multi_polygon poly2;
    bg::read_wkt(second, poly2);

    std::size_t error_count = 0;
    std::size_t n = 0;
    for (int factor = 1; factor < settings.max_factor; factor *= 2)
    {
        std::size_t i = 0;
        double const bound = settings.start_bound / factor;
        double const step = bound / settings.step_factor;
        if (settings.verbose)
        {
            std::cout << "--> use " << bound << " " << step << std::endl;
        }
        for (double offset_x = -bound; offset_x <= bound; offset_x += step, ++i)
        {
            std::size_t j = 0;
            for (double offset_y = -bound; offset_y <= bound; offset_y += step, ++j, ++n)
            {
                n += test_case<OverlayType>(error_count,
                                            case_index, i, j,
                                            min_vertex_index, max_vertex_index,
                                            offset_x, offset_y, expectation,
                                            poly1, poly2, settings);
            }
        }
    }

    std::cout << case_index
            << " #cases: " << n << " #errors: " << error_count << std::endl;
    BOOST_CHECK_EQUAL(error_count, 0u);

    return error_count;
}

int test_main(int argc, char** argv)
{
    BoostGeometryWriteTestConfiguration();
    using coor_t = default_test_type;

    test_settings settings;
    settings.do_output = argc > 2 && atol(argv[2]) == 1;

    // Test three polygons, for the last test two types of intersections
    test_all<coor_t, true, bg::overlay_union>(1, 0, 3, 22.0, settings);
    test_all<coor_t, true, bg::overlay_union>(2, 0, 3, 73.0, settings);
    test_all<coor_t, true, bg::overlay_intersection>(3, 1, 2, 2.0, settings);
    test_all<coor_t, true, bg::overlay_union>(3, 1, 2, 14.0, settings);

    return 0;
}


#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/CompressedImage.h"
#include "sensor_msgs/Image.h"
#include "sensor_msgs/CameraInfo.h"

#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <stdio.h>
#include <iostream>
//#include <Eigen3/Eigen/Dense>

//This node subscribes to /grasseater/camera_node/image/compressed and undistort the image. Then it publish the image to imgpreproc/undist

using namespace cv;
using namespace std;

const double fx=259.5050;
const double fy=263.0909;
const double cx=333.0998;
const double cy=254.7687;
const double s=0.3627;
const double dist[]={-0.2912,0.0630,-0.0051,-0.00062276,0.0017};

//Calibration Parameters
    Mat cameraMatrix = (Mat_<double>(3,3) << fx,s,cx,0.0,fy,cy,0.0,0.0,1.0);
    Mat distParamCV = (Mat_<double>(1,5) << dist[0],dist[1],dist[2],dist[3],dist[4]);
//Create a Mat object to store the undistorted image
    Mat undistImg;

image_transport::Publisher image_pub;

void chatterCallback(sensor_msgs::CompressedImage image_msg)
{
    ros::Time time = ros::Time::now();

    cv_bridge::CvImagePtr cv_ptr;
    cv_ptr = cv_bridge::toCvCopy(image_msg,sensor_msgs::image_encodings::BGR8);

    //Undistort the image
    undistort(cv_ptr->image,undistImg,cameraMatrix,distParamCV);
    //Create a cvImage to transform from OpenCVImage to ImageMsg
    cv_bridge::CvImage cvi_undist;
    cvi_undist.header.stamp = time;
    cvi_undist.header.frame_id = "undist";
    cvi_undist.encoding = "bgr8";
    cvi_undist.image = undistImg;

    //Publish the image
    image_pub.publish(cvi_undist.toImageMsg());

    ROS_INFO_ONCE("First image published");
}

//Transform from world coodinates to image coordinates
/*void w2imCoord(cv::Point2f worldPoint, cv::Point2f &imagePoint, Eigen::MatrixXd homog)
{
    Eigen::Vector3d wPointEig(worldPoint.x,worldPoint.y,1.0);
    Eigen::Vector3d imPointEig;
    imPointEig=homog*wPointEig;
    imPointEig=imPointEig/imPointEig(2);
    imagePoint=Point2f(imPointEig(0),imPointEig(1));
}

//Transform from image coordinates to world coordinates
void im2wCoord(cv::Point2f imagePoint, cv::Point2f &worldPoint, Eigen::MatrixXd homogInv)
{
    Eigen::Vector3d imPointEig(imagePoint.x,imagePoint.y,1.0);
    Eigen::Vector3d wPointEig;
    wPointEig=homogInv*imPointEig;
    wPointEig=wPointEig/wPointEig(2);
    worldPoint=Point2f(wPointEig(0),wPointEig(1));
}*/

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imgpreproc");
  ros::NodeHandle n;
  image_transport::ImageTransport it(n);

  image_pub = it.advertise("/imgpreproc/undist",1);

  ros::Subscriber sub = n.subscribe("/grasseaterpi3/camera_node/image/compressed",1, chatterCallback);

  ros::spin();

  return 0;
}


/*
 * Copyright (c) 2017, The Regents of the University of California (Regents).
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
 * met:
 *
 *    1. Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *
 *    2. Redistributions in binary form must reproduce the above
 *       copyright notice, this list of conditions and the following
 *       disclaimer in the documentation and/or other materials provided
 *       with the distribution.
 *
 *    3. Neither the name of the copyright holder nor the names of its
 *       contributors may be used to endorse or promote products derived
 *       from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS AS IS
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 * Please contact the author(s) of this library if you have any questions.
 * Authors: David Fridovich-Keil   ( dfk@eecs.berkeley.edu )
 */

///////////////////////////////////////////////////////////////////////////////
//
// Defines the GaussianActionValueFunctor class, which derives from the
// ActionValueFunctor base class.
//
///////////////////////////////////////////////////////////////////////////////

#ifndef RL_VALUE_GAUSSIAN_ACTION_VALUE_FUNCTOR_H
#define RL_VALUE_GAUSSIAN_ACTION_VALUE_FUNCTOR_H

#include "../value/continuous_action_value_functor.hpp"
#include "../value/gaussian_params.hpp"
#include "util/types.hpp"

#include <Eigen/Cholesky>
#include <glog/logging.h>
#include <limits>
#include <iostream>
#include <random>
#include <vector>
#include <math.h>

namespace rl {

  template<typename StateType, typename ActionType>
  class GaussianActionValue :
    public ContinuousActionValue<StateType, ActionType> {
  public:
    ~GaussianActionValue() {}

    // Factory method.
    static typename ContinuousActionValue<StateType, ActionType>::Ptr
    Create(const GaussianParams& params);

    // Must implement a deep copy.
    typename ContinuousActionValue<StateType, ActionType>::Ptr Copy() const;

    // Pure virtual method to output the value at a state/action pair.
    double Get(const StateType& state, const ActionType& action) const;

    // Pure virtual method to do a gradient update to underlying weights.
    // Returns average loss.
    double Update(const std::vector<StateType>& states,
                  const std::vector<ActionType>& actions,
                  const std::vector<double>& targets,
                  double step_size);

    // Choose an optimal action in the given state. Returns whether or not
    // optimization was successful.
    bool OptimalAction(const StateType& state, ActionType& action) const;

  private:
    explicit GaussianActionValue(const GaussianParams& params);

    // Covariance kernel function.
    double Kernel(const VectorXd& x, const VectorXd& y) const;

    // Compute the cross covariance vector of a feature vector with
    // the training data.
    void CrossCovariance(const VectorXd& features, VectorXd& cross) const;

    // Helper function to evaluate the GP.
    void Evaluate(const StateType& state, const ActionType& action,
                  double& mean, double& variance) const;

    // Training covariance matrix, with vectors of state/action features
    // training means, and length scales.
    MatrixXd covariance_;
    std::vector<VectorXd> points_;
    VectorXd means_;
    VectorXd lengths_;
    VectorXd squared_lengths_;

    // Fast Cholesky solver.
    Eigen::LLT<MatrixXd> cholesky_;

    // Output of covariance.inv() * means_. Stored for speed.
    VectorXd regressed_means_;

    // Noise variance.
    double noise_variance_;

    // Regularization parameter to trade off mean with variance in the
    // choice of optimal action.
    double regularizer_;

    // Max number of gradient steps to take for choosing the optimal action,
    // with given step size starting from one of 'num_inits_' random initial
    // points. 'epsilon_' is convergence criterion for gradient size.
    const size_t num_inits_;
    const size_t max_steps_;
    const double step_size_;
    const double epsilon_;
  }; //\class GaussianActionValue

// ------------------------------ IMPLEMENTATION ---------------------------- //

  // Factory method.
  template<typename StateType, typename ActionType>
  typename ContinuousActionValue<StateType, ActionType>::Ptr
  GaussianActionValue<StateType, ActionType>::
  Create(const GaussianParams& params) {
    typename ContinuousActionValue<StateType, ActionType>::Ptr
      ptr(new GaussianActionValue<StateType, ActionType>(params));
    return ptr;
  }

  // Must implement a deep copy.
  template<typename StateType, typename ActionType>
  typename ContinuousActionValue<StateType, ActionType>::Ptr
  GaussianActionValue<StateType, ActionType>::Copy() const {
    typename ContinuousActionValue<StateType, ActionType>::Ptr
      ptr(new GaussianActionValue<StateType, ActionType>(*this));
    return ptr;
  }

  // Constructor.
  template<typename StateType, typename ActionType>
  GaussianActionValue<StateType, ActionType>::
  GaussianActionValue(const GaussianParams& params)
    : regularizer_(params.regularizer_),
      noise_variance_(params.noise_variance_),
      max_steps_(params.max_steps_),
      num_inits_(params.num_inits_),
      step_size_(params.step_size_),
      epsilon_(params.epsilon_),
      covariance_(MatrixXd::Zero(params.num_points_, params.num_points_)),
      means_(VectorXd::Zero(params.num_points_)),
      regressed_means_(VectorXd::Zero(params.num_points_)),
      lengths_(params.lengths_),
      squared_lengths_(params.lengths_.cwiseProduct(params.lengths_)),
      ContinuousActionValue<StateType, ActionType>() {
    // Pick random points in the space for training.
    for (size_t ii = 0; ii < params.num_points_; ii++) {
      const StateType state;
      const ActionType action;

      // Unpack into a feature vector.
      VectorXd state_features(StateType::FeatureDimension());
      state.Features(state_features);

      VectorXd action_features(ActionType::FeatureDimension());
      action.Features(action_features);

      VectorXd features(state_features.size() + action_features.size());
      features.head(state_features.size()) = state_features;
      features.tail(action_features.size()) = action_features;

      // Add to list of training points.
      points_.push_back(features);
    }

    // Compute training covariance.
    for (size_t ii = 0; ii < points_.size(); ii++) {
      for (size_t jj = 0; jj < ii; jj++) {
        covariance_(ii, jj) = Kernel(points_[ii], points_[jj]);
        covariance_(jj, ii) = covariance_(ii, jj);
      }

      covariance_(ii, ii) = 1.0 + noise_variance_;
    }

    // Randomize training targets.
    std::random_device rd;
    std::default_random_engine rng(rd());
    std::normal_distribution<double> gaussian(0.0, 0.1);

    for (size_t ii = 0; ii < means_.size(); ii++)
      means_(ii) = gaussian(rng);

    // Compute Cholesky decomposition of 'covariance_' for quick solving.
    cholesky_ = covariance_.llt();

    // Set 'regressed_means_' for speed.
    regressed_means_ = cholesky_.solve(means_);
  }

  // Helper function to evaluate the GP.
  template<typename StateType, typename ActionType>
  void GaussianActionValue<StateType, ActionType>::
  Evaluate(const StateType& state, const ActionType& action,
           double& mean, double& variance) const {
    // Compute cross covariance vector.
    VectorXd features(StateType::FeatureDimension() +
                      ActionType::FeatureDimension());
    this->Unpack(state, action, features);

    VectorXd cross(points_.size());
    CrossCovariance(features, cross);

    // Regress the cross covariance on the training covariance.
    const VectorXd regressed_cross = cholesky_.solve(cross);

    // Compute the total inner product between the cross covariance
    // of this point and the training set.
    mean = cross.dot(regressed_means_);
    variance = 1.0 - cross.dot(regressed_cross);
  }

  // Compute the expected value of the GP at this point.
  template<typename StateType, typename ActionType>
  double GaussianActionValue<StateType, ActionType>::
  Get(const StateType& state, const ActionType& action) const {
    // Compute cross covariance vector.
    VectorXd features(StateType::FeatureDimension() +
                      ActionType::FeatureDimension());
    this->Unpack(state, action, features);

    VectorXd cross(points_.size());
    CrossCovariance(features, cross);

    // Compute the total inner product between the cross covariance
    // of this point and the training set.
    return cross.dot(regressed_means_);
  }

  // Update all parameters. Return average loss.
  template<typename StateType, typename ActionType>
  double GaussianActionValue<StateType, ActionType>::
  Update(const std::vector<StateType>& states,
         const std::vector<ActionType>& actions,
         const std::vector<double>& targets, double step_size) {
    CHECK_EQ(states.size(), actions.size());
    CHECK_EQ(states.size(), targets.size());

    // Iterate over each state/action pair and average the gradients.
    double loss = 0.0;
    VectorXd gradient = VectorXd::Zero(means_.size());
    VectorXd features(StateType::FeatureDimension() +
                      ActionType::FeatureDimension());
    VectorXd cross(points_.size());
    for (size_t ii = 0; ii < states.size(); ii++) {
      // Compute cross covariance.
      this->Unpack(states[ii], actions[ii], features);
      CrossCovariance(features, cross);

      // Compute the gradient.
      const VectorXd regressed = cholesky_.solve(cross);
      const double error = regressed.dot(means_) - targets[ii];

      // Catch nan. Set to zero.
      if (isnan(error)) {
        LOG(WARNING) << "GaussianActionValue: Error was nan. Skipping.";
        continue;
      }

      //      std::printf("Error was: %f = %f - %f\n",
      //                  error, error + targets[ii], targets[ii]);

      loss += error * error;
      gradient += error * regressed;
    }

    // Do a gradient update.
    means_ -= step_size * gradient / static_cast<double>(states.size());

    // Update 'regressed_means_' for speed later.
    regressed_means_ = cholesky_.solve(means_);

    // Return loss. Divide by two for consistency.
    return 0.5 * loss / static_cast<double>(states.size());
  }

  // Compute the optimal action, where 'optimal' is the solution to the
  // following program:
  //                arg max mean(s, a) + reg * var(s, a)
  //                     a
  // Note that the sign of 'regularizer' will determine whether the control
  // is biased toward "safe" areas or optimistic exploration. In practice, this
  // problem is usually non-convex, and we solve it by taking several gradient
  // steps from a few random initializations and returning the best final value.
  template<typename StateType, typename ActionType>
  bool GaussianActionValue<StateType, ActionType>::
  OptimalAction(const StateType& state, ActionType& action) const {
    // Get a list of possible actions.
    std::vector<ActionType> candidates;
    ActionType::DiscreteValues(candidates);

    // Find the best option in this list.
    double max_value = kInvalidValue;
    for (const auto& candidate : candidates) {
      double mean, variance;
      Evaluate(state, candidate, mean, variance);

      const double value = mean + regularizer_ * variance;

      if (value > max_value) {
        max_value = value;
        action = candidate;
      }
    }

    return (max_value != kInvalidValue);

#if 0
    VectorXd features(StateType::FeatureDimension() +
                      ActionType::FeatureDimension());

    // Try a few random initial actions and keep the best one
    // after running a few iterations of gradient ascent.
    double best_value = kInvalidReward;
    bool has_converged = false;

    VectorXd cross(points_.size());
    VectorXd regressed_cross(points_.size());
    MatrixXd Jt(ActionType::FeatureDimension(), points_.size());

    for (size_t ii = 0; ii < num_inits_; ii++) {
      // Start from a random initial action.
      ActionType current_action;
      this->Unpack(state, current_action, features);

      // Run a few steps of gradient ascent to adjust this action.
      has_converged = false;
      for (size_t jj = 0; jj < max_steps_; jj++) {
        // Compute the cross covariance of this state-action pair with the data.
        CrossCovariance(features, cross);

        // Compute the Jacobian transpose of cross covariance with respect
        // to feature vector.
        for (size_t jj = 0; jj < points_.size(); jj++) {
          const VectorXd action_diff =
            points_[jj].tail(ActionType::FeatureDimension()) -
            features.tail(ActionType::FeatureDimension());

          Jt.col(jj) = cross(jj) * action_diff.cwiseQuotient(
            squared_lengths_.tail(ActionType::FeatureDimension()));
        }

        // Compute the intermediate derivative with respect to cross covariance.
        regressed_cross = cholesky_.solve(cross);
        const VectorXd cross_gradient =
          regressed_means_ + regularizer_ * regressed_cross;

        // Compute the gradient with respect to the action feature vector.
        const VectorXd gradient = Jt * cross_gradient;

        // Gradient update.
        features.tail(ActionType::FeatureDimension()) += step_size_ * gradient;

        // Check convergence.
        if (gradient.norm() < epsilon_) {
          has_converged = true;
          break;
        }
      }

      // Check if this value is best.
      const double value =
        (regressed_means_ + regularizer_ * regressed_cross).dot(cross);

      if (value > best_value) {
        best_value = value;
        action.FromFeatures(features.tail(ActionType::FeatureDimension()));
      }
    }

    return has_converged;
#endif
  }

  // Covariance kernel function.
  template<typename StateType, typename ActionType>
  double GaussianActionValue<StateType, ActionType>::
  Kernel(const VectorXd& x, const VectorXd& y) const {
    CHECK_EQ(x.size(), lengths_.size());
    CHECK_EQ(y.size(), lengths_.size());

    const VectorXd normalized_delta = (x - y).cwiseQuotient(lengths_);
    return std::exp(-0.5 * normalized_delta.squaredNorm());
  }

  // Compute the cross covariance vector of a feature vector with
  // the training data.
  template<typename StateType, typename ActionType>
  void GaussianActionValue<StateType, ActionType>::
  CrossCovariance(const VectorXd& features, VectorXd& cross) const {
    CHECK_EQ(cross.size(), points_.size());
    CHECK_EQ(features.size(),
             StateType::FeatureDimension() + ActionType::FeatureDimension());

    // Compute cross covariance of features with training points.
    for (size_t ii = 0; ii < points_.size(); ii++)
      cross(ii) = Kernel(features, points_[ii]);
  }

}  //\namespace rl

#endif


/*
Copyright 2014 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \file
Tests for math::product.
*/

#ifndef MATH_TEST_MATH_PRODUCT_TESTS_HPP_INCLUDED
#define MATH_TEST_MATH_PRODUCT_TESTS_HPP_INCLUDED

#include "math/product.hpp"

#include <string>
#include <vector>

#include <boost/mpl/assert.hpp>

#include "math/arithmetic_magma.hpp"
#include "math/cost.hpp"
#include "math/sequence.hpp"
#include "math/check/check_magma.hpp"

template <class Inverses> void test_product_homogeneous() {
    // Test the empty product.
    // This is (trivially!) almost a semiring.
    // However, it cannot have an annihilator.
    {
        typedef math::product <math::over<>, Inverses> product;
        std::vector <product> examples;
        examples.push_back (product());

        static_assert (!math::has <
                math::callable::annihilator <product> (math::callable::times)
            >::value, "An empty product cannot have an annihilator.");

        math::check_magma <product> (math::times, math::plus, examples);
    }

    // Test homogeneous of (float, sequence).
    {
        typedef math::product <
            math::over <float, math::sequence <char>>, Inverses> product;

        static_assert (math::has <
                math::callable::annihilator <product> (math::callable::times)
            >::value, "An empty product cannot have an annihilator.");

        static_assert (utility::is_assignable <
            typename product::components_type &,
            typename product::components_type &&>::value, "");
        static_assert (utility::is_assignable <
            product &, product &&>::value, "");

        std::vector <product> examples;
        examples.push_back (product (
            0, math::sequence <char> (std::string (""))));
        // This is the same for with_inverse <times>.
        examples.push_back (product (
            0, math::sequence <char> (std::string ("q"))));
        examples.push_back (product (
            4, math::sequence <char> (std::string (""))));
        examples.push_back (product (
            2, math::sequence <char> (std::string ("a"))));
        examples.push_back (product (
            5, math::sequence <char> (std::string ("ab"))));
        examples.push_back (product (
            7, math::sequence <char> (std::string ("cba"))));
        examples.push_back (product (
            3, math::sequence <char> (std::string ("aba"))));
        examples.push_back (product (
            0, math::sequence_annihilator <char> ()));
        examples.push_back (product (
            3, math::sequence_annihilator <char> ()));

        math::check_semiring <product, math::left> (
            math::times, math::plus, examples);
    }
}

template <class Inverses, class Sequence>
    inline math::product <math::over <float, Sequence>, Inverses>
        make_product (float f, Sequence s)
{ return math::product <math::over <float, Sequence>, Inverses> (f, s); }

// Test heterogeneous types.
template <class Inverses> void test_product_heterogeneous() {
    typedef math::product <math::over <float, math::sequence <char>>, Inverses>
        product;
    {
        auto examples = range::make_tuple (
            make_product <Inverses> (2.f, math::empty_sequence <char>()),
            make_product <Inverses> (4.f, math::empty_sequence <char>()),
            make_product <Inverses> (4.f,
                math::sequence <char> (std::string())),
            make_product <Inverses> (5.f, math::single_sequence <char> ('A')),
            make_product <Inverses> (5.f, math::single_sequence <char> ('z')),
            make_product <Inverses> (5.f,
                math::sequence <char> (std::string("z"))),
            make_product <Inverses> (1.f,
                math::sequence <char> (std::string("Az"))),
            make_product <Inverses> (1.f,
                math::sequence <char> (std::string("zAz")))
            );

        math::check_semiring <product, math::left> (
            math::times, math::plus, examples);
    }
}

#endif // MATH_TEST_MATH_PRODUCT_TESTS_HPP_INCLUDED


#include <string>
#include <map>
#include <fstream>
#include <boost/multiprecision/cpp_int.hpp>
#include <boost/crc.hpp>

using namespace std;
using namespace boost;
using namespace boost::multiprecision;
using u256 = uint256_t;

namespace {
	enum class HashAlgorithm
	{
		DJB2,
		CRC32
	};
	string hashAlgName(HashAlgorithm alg)
	{
		switch (alg)
		{
			case HashAlgorithm::DJB2:
				return "DJB2";
			case HashAlgorithm::CRC32:
				return "CRC32";
		}
	}
}

static map<u256, pair<string, HashAlgorithm>> s_hashMap = {};

struct NCHFCollision
{
	string printStringAsHex(string const& input)
	{
		ostringstream os;
		for (auto const& c: input)
			os << hex << setfill('0') << setw(2) << right << int(c);
		return os.str();
	}

	void printCollision(HashAlgorithm alg, u256 hash, string input)
	{
		// Append to a local file in the same directory
		// as the fuzzer binary
		ofstream f("NCHF-Collision.txt", ios::app);
		f << endl;
		f << "Algorithm: " << hashAlgName(alg) << endl;
		f << "Hash: " << hash << endl;
		f << "Input 1: " << printStringAsHex(s_hashMap[hash].first) << endl;
		f << "Input 2: " << printStringAsHex(input) << endl;
	}

	bool operator()(HashAlgorithm alg, u256 hash, string input)
	{
		bool Collision = s_hashMap.count(hash) &&
			input != s_hashMap[hash].first &&
			alg == s_hashMap[hash].second;

		if (Collision)
		{
			printCollision(alg, hash, input);
			return true;
		}
		else
		{
			s_hashMap.emplace(hash, make_pair(input, alg));
			return false;
		}
	}
};

struct NCHF
{
	bool operator()(const uint8_t *data, size_t size, HashAlgorithm alg)
	{
		return NCHFCollision{}(
			alg,
			computeHash(data, size),
			string(data, data + size)
		);
	}

	virtual u256 computeHash(const uint8_t *data, size_t size) = 0;
};

struct DJB2: NCHF
{
	u256 computeHash(const uint8_t *data, size_t size) override
	{
		string input(data, data + size);
		u256 hash = 5381;
		for (auto c: input)
			hash = (hash << 5) + hash + c;
		return hash;
	}
};

struct CRC32: NCHF
{
	u256 computeHash(const uint8_t *data, size_t size) override
	{
		string input(data, data + size);
		crc_32_type hash;
		hash.process_bytes(input.c_str(), input.size());
		return hash.checksum();
	}
};

extern "C" int LLVMFuzzerTestOneInput(const uint8_t* data, size_t size)
{
//	assert(!DJB2{}(data, size, HashAlgorithm::DJB2));
	assert(!CRC32{}(data, size, HashAlgorithm::CRC32));
	return 0;
}


///////////////////////////////////////////////////////////////////////////////
// statistics::survival::model::example::model::exponential.cpp              //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#include <stdexcept>
#include <string> //needed?
#include <vector>
#include <limits>
#include <ostream>
#include <fstream>
#include <algorithm>
#include <iterator>
// #include <boost/archive/binary_oarchive.hpp>
// #include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/serialization/vector.hpp>

#include <boost/arithmetic/equal.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/iterator/range_cycle.hpp>
#include <boost/range.hpp>
#include <boost/assert.hpp>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/iterator/range_cycle.hpp>

#include <boost/standard_distribution/distributions/normal.hpp>

#include <boost/statistics/survival/data/include.hpp>
#include <boost/statistics/survival/model/models/exponential/include.hpp>
#include <libs/statistics/survival/model/example/exponential.h>

// Must come after the model to be used
#include <boost/statistics/model/include.hpp>

#include <libs/statistics/survival/model/example/exponential.h>

void example_exponential(std::ostream& out){

    out << "-> example_model_exponential : ";

    // Steps shown in this example:
    //
    // Loads the first batch of a set of records
    // Creates events at given time
    // Evaluates the likelihoods and posteriors

    using namespace boost;
    using namespace statistics;
    namespace surv = survival;

    // [ Types ]
    // Value
    typedef double                                  val_;
    typedef std::vector<val_>                       vals_;
    typedef surv::constant<val_>                    const_;

    // I/O
    typedef boost::archive::text_oarchive oa_;
    typedef boost::archive::text_iarchive ia_;

   // Records
    typedef surv::data::record<val_>                record_;
    typedef std::vector<record_>                    records_;
    typedef range_iterator<records_>::type          it_record_;

    // Events
    typedef surv::data::event<val_>                 event_;
    typedef std::vector<event_>                     events_;
    typedef range_iterator<events_>::type           it_event_;

    // Covariates
    typedef val_                                    x_;
    typedef vals_                                   r_x_;
    typedef range_cycle<>                           range_cycle_;
    typedef range_cycle_::apply<r_x_>::type         x_cycle_;

    // Model
    typedef surv::model::exponential::model<val_>   model_;
    typedef val_                                    par_;
    typedef vals_                                   pars_;

    // [ Constants ]
    const val_ entry_bound = const_::inf_;
    const val_ par = 2.0;
    const char* batches_path
        = "/Users/erwann/projets/2009/Xcode/survival/build/Release/batches";

    // [ Variables ]
    long n_record;

    // [ Upload first batch of records ]
    records_ records;
    {
        std::ifstream ifs(batches_path);
        if(ifs.good()){
            ia_ ia(ifs);
            ia >> records;
        }else{
            std::string str = "error opening : ";
            str.append( batches_path );
            throw std::runtime_error(str);
        }
        ifs.close();
    }
    n_record = boost::size( records );

    // [ Events ]
    events_ events;
    events.reserve( size(records) );
    surv::data::events(
        begin(records),
        end(records),
        entry_bound,
        std::back_inserter(events)
    );

    // [ Covariates ]
    r_x_ r_x;
    {
        using namespace boost::assign;
        r_x += -0.5, 0.5;
    }
    x_cycle_ x_cycle = range_cycle_::make(r_x,0,n_record);
    out << "size(x_cycle) = " << size(x_cycle) << std::endl;
    BOOST_ASSERT( size(x_cycle)>=size(events) );
    // Resize x_cycle to a size that matches that of events
    x_cycle.advance_end( - (size(x_cycle) - size(events)) );
    BOOST_ASSERT( size(x_cycle) == size(events) );

    // Model
    model_ model;

    // Pars
    pars_ pars;
    {
        using namespace assign;
        pars += -2.0, -1.0, 0.0, 1.0, 2.0;
    }

    // [ Likelihood ]

    typedef math::normal_distribution<val_> mprior_;
    mprior_ mprior;

    out << '(';
    out << model::log_likelihood<val_>(
        model::make_model_data(
            model,
            r_x[0],
            events[0]
        ),
        par
    );
    out << ',';
    out << model::log_likelihood<val_>(
        model::make_model_data(
            model,
            r_x[1],
            events[1]
        ),
        par
    ) << ')';

    // [ Likelihoods ]
    vals_ lls;
    model::log_likelihoods<val_>(
        model::make_model_dataset(model,r_x,events),
        boost::begin(pars),
        boost::end(pars),
        std::back_inserter(lls)
    );

    // [ Prior ]
    vals_ lprs;
    math::transform<math::fun_wrap::log_unnormalized_pdf_>(
        mprior,
        boost::begin(pars),
        boost::end(pars),
        std::back_inserter(lprs)
    );

    // [ Posteriors ]

    vals_ lpos;
    model::log_posteriors2<val_>(
        model::make_prior_model_dataset(mprior,model,r_x,events),
        boost::begin(pars),
        boost::end(pars),
        std::back_inserter(lpos)
    );

    // Consistency check
    typedef range_iterator<vals_>::type it_val_;
    {

        it_val_ i_lpr = boost::begin(lprs);
        it_val_ i_lpo = boost::begin(lpos);
        out << std::endl;
        out << "log(prior,likelihood,posterior)" << std::endl;
        for(
            it_val_ i_ll = boost::begin(lls);
            i_ll< boost::end(lls);
            i_ll++,i_lpr++,i_lpo++
        ){
            out << '(';
            val_ lpr = *i_lpr;  out << lpr << ',';
            val_ ll = *i_ll;    out << ll << ',';
            val_ lpo = *i_lpo;  out << lpo << ')' << std::endl;
            val_ lpo2 = lpr + ll;
            BOOST_ASSERT(
                arithmetic_tools::equal(
                    lpo,
                    lpo2
                )
            );
        }
    }

    // Consistency check2

    {
        vals_ lpr2s ( size(pars) );
        model::log_posteriors<val_>(
            model::make_prior_model_dataset(mprior,model,r_x,events),
            boost::begin(pars),
            boost::end(pars),
            boost::begin(lls), //subtracted
            boost::begin(lpr2s)
        );

        it_val_ i_lpr = boost::begin(lprs);
        out << std::endl;
        out << "log(prior,prior2)" << std::endl;
        for(
            it_val_ i_lpr2 = boost::begin(lpr2s);
            i_lpr2< boost::end(lpr2s);
            i_lpr2++,i_lpr++
        ){
            out << '(';
            val_ lpr  = *i_lpr;     out << lpr << ',';
            val_ lpr2 = *i_lpr2;    out << lpr2 << ')' << std::endl;
            BOOST_ASSERT(
                arithmetic_tools::equal(
                    lpr,
                    lpr2
                )
            );
        }
    }

    out << "<-" << std::endl;
}


#include <algorithm>
#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <iostream>
#include <list>
#include <map>
#include <set>
#include <tbb/blocked_range.h>
#include <tbb/parallel_for.h>
#include <vector>

struct Layer
{
    int range = 0;
    std::vector<int> positions;

    Layer(int r)
        : range(r)
    {
        for (int i = 0; i < r; ++i) {
            positions.push_back(i);
        }
        for (int i = range - 2; i > 0; --i) {
            positions.push_back(i);
        }
    }

    int scanner_pos(int tick) const
    {
        return positions[tick % positions.size()];
    }
};

using Map = std::map<int, Layer>;

int main()
{
    Map map;

    std::string line;
    int max_k = 0;
    while (std::getline(std::cin, line)) {
        std::vector<std::string> words;
        boost::algorithm::split(
                words, line, boost::is_any_of(": "), boost::algorithm::token_compress_on);

        int k = std::stoi(&words[0][0]);
        int r = std::stoi(&words[1][0]);
        map.insert({k, Layer{r}});
        max_k = std::max(max_k, k);
    }

    int delay = 0;
    for (;;++delay) {
        bool hit = false;
        for (int pos = 0; pos <= max_k; ++pos) {
            int tick = delay + pos;
            auto it = map.find(pos);
            if (it != map.end()) {
                if (it->second.scanner_pos(tick) == 0) {
                    hit = true;
                    break;
                }
            }
        }
        if (!hit) {
            break;
        }
    }

    std::cout << "delay: " << delay << "\n";

    return 0;
}


#ifndef PYTHONIC_INCLUDE_NUMPY_LOG_HPP
#define PYTHONIC_INCLUDE_NUMPY_LOG_HPP

#include "pythonic/include/utils/functor.hpp"
#include "pythonic/include/types/ndarray.hpp"
#include "pythonic/include/utils/numpy_traits.hpp"

#include <boost/simd/function/log.hpp>

PYTHONIC_NS_BEGIN

namespace numpy
{
  namespace wrapper
  {
    template <class T>
    std::complex<T> log(std::complex<T> const &val)
    {
      return std::log(val);
    }

    template <class T>
    typename std::enable_if<std::is_integral<T>::value, double>::type
    log(T const &val)
    {
      return std::log(val);
    }

    template <class T>
    auto log(T const &val) ->
        typename std::enable_if<!std::is_integral<T>::value,
                                decltype(boost::simd::log(val))>::type
    {
      return boost::simd::log(val);
    }
  }
#define NUMPY_NARY_FUNC_NAME log
#define NUMPY_NARY_FUNC_SYM wrapper::log
#include "pythonic/include/types/numpy_nary_expr.hpp"
}
PYTHONIC_NS_END

#endif


/*****************************************************************************
*
* Rokko: Integrated Interface for libraries of eigenvalue decomposition
*
* Copyright (C) 2013-2015 Rokko Developers https://github.com/t-sakashita/rokko
*
* Distributed under the Boost Software License, Version 1.0. (See accompanying
* file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)
*
*****************************************************************************/

#include <rokko/rokko.hpp>
#include <rokko/utility/frank_matrix.hpp>
#include <boost/lexical_cast.hpp>
#define BOOST_TEST_MODULE test_product
#ifndef BOOST_TEST_DYN_LINK
#include <boost/test/included/unit_test.hpp>
#else
#include <boost/test/unit_test.hpp>
#endif

BOOST_AUTO_TEST_CASE(test_product) {
  MPI_Init(&boost::unit_test::framework::master_test_suite().argc,
           &boost::unit_test::framework::master_test_suite().argv);
  MPI_Comm comm = MPI_COMM_WORLD;
  int rank;
  MPI_Comm_rank(comm, &rank);

  int dim = 100;
  if (boost::unit_test::framework::master_test_suite().argc > 1) {
    dim = boost::lexical_cast<int>(boost::unit_test::framework::master_test_suite().argv[1]);
  }

  if (rank == 0) std::cout << "dimension = " << dim << std::endl;
  rokko::parallel_dense_solver solver(rokko::parallel_dense_solver::default_solver());
  rokko::grid g(comm);
  rokko::distributed_matrix<double, rokko::matrix_col_major> matA(dim, dim, g, solver);
  rokko::distributed_matrix<double, rokko::matrix_col_major> matB(dim, dim, g, solver);
  rokko::distributed_matrix<double, rokko::matrix_col_major> matC(dim, dim, g, solver);
  rokko::frank_matrix::generate(matA);
  rokko::frank_matrix::generate(matB);
  rokko::product(1.0, matA, false, matB, false, 0, matC);
  matC.print();
  // calculate trace
  double sum_local = 0;
  for (int i = 0; i < dim; ++i) {
    if (matC.is_gindex(i, i)) sum_local += matC.get_global(i, i);
  }
  double sum_global = 0;
  MPI_Allreduce(&sum_local, &sum_global, 1, MPI_DOUBLE, MPI_SUM, comm);
  if (rank == 0) std::cout << "trace of distributed matrix = " << sum_global << std::endl;

  rokko::localized_matrix<double, rokko::matrix_col_major> lmatA(dim, dim);
  rokko::frank_matrix::generate(lmatA);
  rokko::localized_matrix<double, rokko::matrix_col_major> lmatC = lmatA * lmatA;
  if (rank == 0) std::cout << lmatC << std::endl;
  // calculate trace
  double sum = 0;
  for (int i = 0; i < dim; ++i) {
    sum += lmatC(i, i);
  }
  if (rank == 0) std::cout << "trace of localized matrix = " << sum << std::endl;

  if (rank == 0) BOOST_CHECK_CLOSE(sum_global, sum, 10e-12);

  MPI_Finalize();
}


#include <iostream>
#include <limits>

#include <boost/rational.hpp>
#include <boost/safe_numerics/safe_integer.hpp>

int main(int, const char *[]){
    // simple demo of rational library
    const boost::rational<int> r {1, 2};
    std::cout << "r = " << r << std::endl;
    const boost::rational<int> q {-2, 4};
    std::cout << "q = " << q << std::endl;
    // display the product
    std::cout << "r * q = " << r * q << std::endl;

    // problem: rational doesn't handle integer overflow well
    const boost::rational<int> c {1, INT_MAX};
    std::cout << "c = " << c << std::endl;
    const boost::rational<int> d {1, 2};
    std::cout << "d = " << d << std::endl;
    // display the product - wrong answer
    std::cout << "c * d = " << c * d << std::endl;

    // solution: use safe integer in rational definition
    using safe_rational = boost::rational<
        boost::safe_numerics::safe<int>
    >;

    // use rationals created with safe_t
    const safe_rational sc {1, std::numeric_limits<int>::max()};

    std::cout << "c = " << sc << std::endl;
    const safe_rational sd {1, 2};
    std::cout << "d = " << sd << std::endl;
    std::cout << "c * d = ";
    try {
        // multiply them. This will overflow
        std::cout << sc * sd << std::endl;
    }
    catch (std::exception const& e) {
        // catch exception due to multiplication overflow
        std::cout << e.what() << std::endl;
    }

    return 0;
}


#include <stan/math/prim/scal.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#include <gtest/gtest.h>

TEST(MathFunctions, Phi_approx) {
  EXPECT_EQ(0.5, stan::math::Phi_approx(0.0));
  EXPECT_NEAR(stan::math::Phi(0.9), stan::math::Phi_approx(0.9), 0.00014);
  EXPECT_NEAR(stan::math::Phi(-5.0), stan::math::Phi_approx(-5.0), 0.00014);
}

TEST(MathFunctions, Phi_approx_nan) {
  double nan = std::numeric_limits<double>::quiet_NaN();

  EXPECT_PRED1(boost::math::isnan<double>,
               stan::math::Phi_approx(nan));
}


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/assert.hpp>
#include <boost/hana/concept/ring.hpp>
#include <boost/hana/mult.hpp>
#include <boost/hana/one.hpp>
#include <boost/hana/tuple.hpp>

#include <laws/ring.hpp>
namespace hana = boost::hana;

int main() {
    hana::test::TestRing<int>{hana::make_tuple(0,1,2,3,4,5)};
    hana::test::TestRing<long>{hana::make_tuple(0l,1l,2l,3l,4l,5l)};

    // one
    static_assert(hana::one<int>() == 1, "");

    // mult
    static_assert(hana::mult(6, 4) == 6 * 4, "");
}


#define CATCH_CONFIG_ENABLE_BENCHMARKING

#include <Eigen/Dense>
#include <catch2/catch.hpp>
#include <unsupported/Eigen/CXX11/Tensor>

#include "../src/kernel.hpp"

using Point3 = Eigen::Matrix<float, 3, 1>;

template <int IP, int TP>
Eigen::TensorFixedSize<float, Eigen::Sizes<IP, IP, TP>> DistSq(Point3 const p)
{
  using KTensor = Eigen::TensorFixedSize<float, Eigen::Sizes<IP, IP, TP>>;
  using KArray = Eigen::TensorFixedSize<float, Eigen::Sizes<IP>>;
  using FixIn = Eigen::type2index<IP>;
  KArray indices;
  std::iota(indices.data(), indices.data() + IP, -IP / 2); // Note INTEGER division
  KTensor k;
  if constexpr (TP > 1) {
    constexpr Eigen::IndexList<FixIn, FixOne, FixOne> rshX;
    constexpr Eigen::IndexList<FixOne, FixIn, FixIn> brdX;
    constexpr Eigen::IndexList<FixOne, FixIn, FixOne> rshY;
    constexpr Eigen::IndexList<FixIn, FixOne, FixIn> brdY;
    constexpr Eigen::IndexList<FixOne, FixOne, FixIn> rshZ;
    constexpr Eigen::IndexList<FixIn, FixIn, FixOne> brdZ;
    auto const kx = ((indices.constant(p[0]) - indices) / indices.constant(IP / 2.f))
                      .square()
                      .reshape(rshX)
                      .broadcast(brdX);
    auto const ky = ((indices.constant(p[1]) - indices) / indices.constant(IP / 2.f))
                      .square()
                      .reshape(rshY)
                      .broadcast(brdY);
    auto const kz = ((indices.constant(p[2]) - indices) / indices.constant(TP / 2.f))
                      .square()
                      .reshape(rshZ)
                      .broadcast(brdZ);
    k = kx + ky + kz;
  } else {
    constexpr Eigen::IndexList<FixIn, FixOne, FixOne> rshX;
    constexpr Eigen::IndexList<FixOne, FixIn, FixOne> brdX;
    constexpr Eigen::IndexList<FixOne, FixIn, FixOne> rshY;
    constexpr Eigen::IndexList<FixIn, FixOne, FixOne> brdY;
    auto const kx = ((indices.constant(p[0]) - indices) / indices.constant(IP / 2.f))
                      .square()
                      .reshape(rshX)
                      .broadcast(brdX);
    auto const ky = ((indices.constant(p[1]) - indices) / indices.constant(IP / 2.f))
                      .square()
                      .reshape(rshY)
                      .broadcast(brdY);
    k = kx + ky;
  }
  return k;
}

template <int IP, int TP>
Eigen::TensorFixedSize<float, Eigen::Sizes<IP, IP, TP>> Naive(Point3 const p)
{
  using KTensor = Eigen::TensorFixedSize<float, Eigen::Sizes<IP, IP, TP>>;
  KTensor k;
  for (Index iz = 0; iz < TP; iz++) {
    for (Index iy = 0; iy < IP; iy++) {
      for (Index ix = 0; ix < IP; ix++) {
        k(ix, iy, iz) = ((p - Point3(ix, iy, iz)) / (IP / 2.f)).squaredNorm();
      }
    }
  }
  return k;
}

TEST_CASE("Kernels")
{
  auto const z = Point3::Zero();
  KaiserBessel<3, 3> kb(2.f);
  FlatIron<3, 3> fi(2.f);

  BENCHMARK("Old")
  {
    DistSq<3, 3>(z);
  };

  BENCHMARK("Current")
  {
    kb.distSq(z);
  };

  BENCHMARK("Naive")
  {
    Naive<3, 3>(z);
  };

  BENCHMARK("KB")
  {
    kb.k(z);
  };

  BENCHMARK("FI")
  {
    fi.k(z);
  };
}