#ifndef STAN_MATH_PRIM_SCAL_PROB_POISSON_LCDF_HPP
#define STAN_MATH_PRIM_SCAL_PROB_POISSON_LCDF_HPP

#include <stan/math/prim/scal/meta/is_constant_struct.hpp>
#include <stan/math/prim/scal/meta/partials_return_type.hpp>
#include <stan/math/prim/scal/meta/operands_and_partials.hpp>
#include <stan/math/prim/scal/meta/scalar_seq_view.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_less.hpp>
#include <stan/math/prim/scal/err/check_nonnegative.hpp>
#include <stan/math/prim/scal/err/check_not_nan.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/fun/multiply_log.hpp>
#include <stan/math/prim/scal/fun/gamma_q.hpp>
#include <stan/math/prim/scal/fun/lgamma.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>
#include <cmath>
#include <limits>

namespace stan {
  namespace math {

    template <typename T_n, typename T_rate>
    typename return_type<T_rate>::type
    poisson_lcdf(const T_n& n, const T_rate& lambda) {
      static const char* function("poisson_lcdf");
      typedef typename stan::partials_return_type<T_n, T_rate>::type
        T_partials_return;

      if (!(stan::length(n) && stan::length(lambda)))
        return 0.0;

      T_partials_return P(0.0);

      check_not_nan(function, "Rate parameter", lambda);
      check_nonnegative(function, "Rate parameter", lambda);
      check_consistent_sizes(function,
                             "Random variable", n,
                             "Rate parameter", lambda);

      scalar_seq_view<T_n> n_vec(n);
      scalar_seq_view<T_rate> lambda_vec(lambda);
      size_t size = max_size(n, lambda);

      using std::log;
      using std::exp;

      operands_and_partials<T_rate> ops_partials(lambda);

      // Explicit return for extreme values
      // The gradients are technically ill-defined, but treated as neg infinity
      for (size_t i = 0; i < stan::length(n); i++) {
        if (value_of(n_vec[i]) < 0)
          return ops_partials.build(negative_infinity());
      }

      for (size_t i = 0; i < size; i++) {
        // Explicit results for extreme values
        // The gradients are technically ill-defined, but treated as zero
        if (value_of(n_vec[i]) == std::numeric_limits<int>::max())
          continue;

        const T_partials_return n_dbl = value_of(n_vec[i]);
        const T_partials_return lambda_dbl = value_of(lambda_vec[i]);
        const T_partials_return log_Pi = log(gamma_q(n_dbl + 1, lambda_dbl));

        P += log_Pi;

        if (!is_constant_struct<T_rate>::value)
          ops_partials.edge1_.partials_[i] += -exp(n_dbl * log(lambda_dbl)
                                                   - lambda_dbl
                                                   - lgamma(n_dbl + 1)
                                                   - log_Pi);
      }
      return ops_partials.build(P);
    }

  }
}
#endif


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

#ifndef maths_hpp
#define maths_hpp

#include <vector>
#include <cstddef>
#include <cmath>
#include <numeric>
#include <algorithm>
#include <type_traits>
#include <iterator>
#include <functional>
#include <limits>
#include <utility>
#include <stdexcept>
#include <cassert>

#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/factorials.hpp>
#include <boost/math/special_functions/sign.hpp>
#include <boost/math/special_functions/digamma.hpp>
#include <boost/math/special_functions/beta.hpp>
#include <boost/math/distributions/beta.hpp>
#include <boost/math/distributions/geometric.hpp>
#include <boost/math/distributions/binomial.hpp>
#include <boost/math/distributions/normal.hpp>

#include "fmath.hpp"

namespace octopus { namespace maths {

namespace constants
{
    template <typename T = double>
    constexpr T ln10Div10 = T {0.230258509299404568401799145468436420760110148862877297603};
}

template <typename RealType>
bool is_subnormal(const RealType x) noexcept
{
    return std::fpclassify(x) == FP_SUBNORMAL;
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType round(const RealType val, const unsigned precision = 2)
{
    const auto factor = std::pow(RealType {10.0}, precision);
    return std::round(val * factor) / factor;
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
T round_sf(const T x, const int n)
{
    // https://stackoverflow.com/a/13094362/2970186
    if (x == 0.0) return 0;
    auto factor = std::pow(10.0, n - std::ceil(std::log10(std::abs(x))));
    return std::round(x * factor) / factor;
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
bool almost_equal(const T lhs, T rhs, const int ulp = 1)
{
    return lhs == rhs || std::abs(lhs - rhs) < std::numeric_limits<T>::epsilon() * std::abs(lhs + rhs) * ulp;
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
bool almost_zero(const T x, const int ulp = 1)
{
    return almost_equal(x, T {0}, ulp);
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
bool almost_one(const T x, const int ulp = 1)
{
    return almost_equal(x, T {1}, ulp);
}

template <typename T, typename = typename std::enable_if_t<std::is_floating_point<T>::value>>
int count_leading_zeros(const T x)
{
    if (x == 0.0) return 0;
    return -std::ceil(std::log10(std::abs(x - std::numeric_limits<T>::epsilon())));
}

template <typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
constexpr IntegerType ipow(const IntegerType base, const IntegerType exponent) noexcept
{
    if (exponent == 0) return 1;
    if (exponent == 1) return base;
    const auto y = ipow(base, exponent / 2);
    return exponent % 2 == 0 ? y * y : base * y * y;
}

template <typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
bool is_safe_ipow(const IntegerType base, const IntegerType exponent) noexcept
{
    return exponent * std::log(base) <= std::log(std::numeric_limits<IntegerType>::max());
}

template <typename RealType>
constexpr RealType exp_maclaurin(const RealType x)
{
    return (6 + x * (6 + x * (3 + x))) * 0.16666666;
}

template <typename RealType>
constexpr RealType mercator(const RealType x)
{
    return x - x * x / 2 + x * x * x / 3;
}

struct IdFunction
{
    template <typename T>
    const T& operator()(const T& x) const noexcept { return x; }
};

template <typename RealType = double, typename InputIt, typename UnaryOperation>
auto mean(InputIt first, InputIt last, UnaryOperation unary_op)
{
    return std::accumulate(first, last, RealType {0},
                           [&] (const auto curr, const auto& x) {
                               return curr + unary_op(x);
                           }) / std::distance(first, last);
}

template <typename InputIt>
auto mean(InputIt first, InputIt last)
{
    return mean(first, last, IdFunction {});
}

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

template <typename Container, typename UnaryOperation>
auto mean(const Container& values, UnaryOperation unary_op)
{
    return mean(std::cbegin(values), std::cend(values), unary_op);
}

namespace detail {

template <typename T = double, typename ForwardIt>
T median_unsorted(ForwardIt first, ForwardIt last)
{
    const auto n = std::distance(first, last);
    assert(n > 0);
    if (n == 1) return *first;
    if (n == 2) return static_cast<T>(*first + *std::next(first)) / 2;
    const auto middle = std::next(first, n / 2);
    std::nth_element(first, middle, last);
    if (n % 2 == 1) {
        return *middle;
    } else {
        auto prev_middle_itr = std::max_element(first, middle);
        return static_cast<T>(*prev_middle_itr + *middle) / 2;
    }
}

template <typename T = double, typename ForwardIt>
T median_sorted(ForwardIt first, ForwardIt last)
{
    const auto n = std::distance(first, last);
    assert(n > 0);
    if (n == 1) return *first;
    const auto middle = std::next(first, n / 2);
    if (n % 2 == 1) {
        return *middle;
    } else {
        return static_cast<T>(*std::prev(middle) + *middle) / 2;
    }
}

template <typename T = double, typename ForwardIt>
T median_const(ForwardIt first, ForwardIt last)
{
    if (std::is_sorted(first, last)) {
        return median_sorted<T>(first, last);
    } else {
        std::vector<typename std::iterator_traits<ForwardIt>::value_type> tmp {first, last};
        return median_unsorted<T>(std::begin(tmp), std::end(tmp));
    }
}

} // namespace detail

template <typename T = double, typename ForwardIt>
T median(ForwardIt first, ForwardIt last)
{
    return detail::median_unsorted<T>(first, last);
}

template <typename T = double, typename Range>
T median(Range& values)
{
    return median<T>(std::begin(values), std::end(values));
}

template <typename T = double, typename Range>
T median(const Range& values)
{
    return detail::median_const<T>(std::cbegin(values), std::cend(values));
}

template <typename ForwardIterator, typename UnaryOperation>
auto stdev(ForwardIterator first, ForwardIterator last, UnaryOperation unary_op)
{
    const auto m = mean(first, last, unary_op);
    const auto n = std::distance(first, last);
    const auto sum_square = [&] (auto total, const auto& x) { return total + std::pow(unary_op(x) - m, 2); };
    const auto ss = std::accumulate(first, last, 0.0, sum_square);
    return std::sqrt(ss / n);
}

template <typename ForwardIterator>
auto stdev(ForwardIterator first, ForwardIterator last)
{
    return stdev(first, last, IdFunction {});
}

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

template <typename Container, typename UnaryOperation>
auto stdev(const Container& values, UnaryOperation unary_op)
{
    return stdev(std::cbegin(values), std::cend(values), unary_op);
}

template <typename RealType = double, typename InputIt>
RealType rmq(InputIt first, InputIt last)
{
    if (first == last) return 0.0;
    return std::sqrt((std::inner_product(first, last, first, RealType {0}))
                     / static_cast<RealType>(std::distance(first, last)));
}

template <typename RealType = double, typename Container>
RealType rmq(const Container& values)
{
    return rmq<RealType>(std::cbegin(values), std::cend(values));
}

inline float fast_exp(const float x) noexcept
{
    return fmath::exp(x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_exp(const RealType x) noexcept
{
    return std::exp(x);
}

inline float fast_log(const float x) noexcept
{
    return fmath::log(x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log(const RealType x) noexcept
{
    return std::log(x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const RealType a, const RealType b)
{
    const auto r = std::minmax(a, b);
    return r.second + std::log(RealType {1} + std::exp(r.first - r.second));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const RealType a, const RealType b, const RealType c)
{
    const auto max = std::max({a, b, c});
    return max + std::log(std::exp(a - max) + std::exp(b - max) + std::exp(c - max));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(std::initializer_list<RealType> il)
{
    const auto max = std::max(il);
    return max + std::log(std::accumulate(std::cbegin(il), std::cend(il), RealType {0},
                                          [max] (const auto curr, const auto x) {
                                              return curr + std::exp(x - max);
                                          }));
}

template <typename ForwardIt,
          typename = std::enable_if_t<!std::is_floating_point<ForwardIt>::value>>
inline auto log_sum_exp(ForwardIt first, ForwardIt last)
{
    assert(first != last);
    using RealType = typename std::iterator_traits<ForwardIt>::value_type;
    const auto max = *std::max_element(first, last);
    return max + std::log(std::accumulate(first, last, RealType {0},
                                          [max] (const auto curr, const auto x) {
                                              return curr + std::exp(x - max);
                                          }));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const std::array<RealType, 1>& logs)
{
    return logs[0];
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const std::array<RealType, 2>& logs)
{
    return log_sum_exp(logs[0], logs[1]);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType log_sum_exp(const std::array<RealType, 3>& logs)
{
    return log_sum_exp(logs[0], logs[1], logs[2]);
}

template <typename Container>
inline auto log_sum_exp(const Container& values)
{
    return log_sum_exp(std::cbegin(values), std::cend(values));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const RealType a, const RealType b) noexcept
{
    const auto r = std::minmax(a, b);
    return r.second + fast_log(RealType {1} + fast_exp(r.first - r.second));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const RealType a, const RealType b, const RealType c) noexcept
{
    const auto max = std::max({a, b, c});
    return max + fast_log(fast_exp(a - max) + fast_exp(b - max) + fast_exp(c - max));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(std::initializer_list<RealType> il) noexcept
{
    const auto max = std::max(il);
    return max + fast_log(std::accumulate(std::cbegin(il), std::cend(il), RealType {0},
                                          [max] (const auto curr, const auto x) noexcept {
                                              return curr + fast_exp(x - max);
                                          }));
}

template <typename ForwardIt,
          typename = std::enable_if_t<!std::is_floating_point<ForwardIt>::value>>
inline auto fast_log_sum_exp(ForwardIt first, ForwardIt last) noexcept
{
    assert(first != last);
    using RealType = typename std::iterator_traits<ForwardIt>::value_type;
    const auto max = *std::max_element(first, last);
    return max + fast_log(std::accumulate(first, last, RealType {0},
                                          [max] (const auto curr, const auto x) noexcept {
                                              return curr + fast_exp(x - max);
                                          }));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const std::array<RealType, 1>& logs) noexcept
{
    return logs[0];
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const std::array<RealType, 2>& logs) noexcept
{
    return fast_log_sum_exp(logs[0], logs[1]);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
inline RealType fast_log_sum_exp(const std::array<RealType, 3>& logs) noexcept
{
    return fast_log_sum_exp(logs[0], logs[1], logs[2]);
}

template <typename Container>
inline auto fast_log_sum_exp(const Container& values) noexcept
{
    return fast_log_sum_exp(std::cbegin(values), std::cend(values));
}

template <typename T, typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
T factorial(const IntegerType x)
{
    return boost::math::factorial<double>(x);
}

template <typename RealType, typename IntegerType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>>
RealType log_factorial(IntegerType x)
{
    return std::lgamma(x + 1);
}

template <typename InputIt, typename Log>
auto entropy(InputIt first, InputIt last, Log&& log)
{
    using RealType = typename std::iterator_traits<InputIt>::value_type;
    static_assert(std::is_floating_point<RealType>::value,
                  "entropy is only defined for floating point values");
    const auto add_entropy = [&] (auto total, auto p) { return total + (p > 0 ? p * log(p) : 0); };
    return -std::accumulate(first, last, RealType {0}, add_entropy);
}

template <typename Range, typename Log>
auto entropy(const Range& values, Log&& log)
{
    return entropy(std::cbegin(values), std::cend(values), std::forward<Log>(log));
}

template <typename InputIt>
auto entropy(InputIt first, InputIt last)
{
    return entropy(first, last, [] (auto x) { return std::log(x); });
}

template <typename Range>
auto entropy(const Range& values)
{
    return entropy(std::cbegin(values), std::cend(values));
}

template <typename InputIt>
auto entropy2(InputIt first, InputIt last)
{
    return entropy(first, last, [] (auto x) { return std::log2(x); });
}

template <typename Range>
auto entropy2(const Range& values)
{
    return entropy2(std::cbegin(values), std::cend(values));
}

template <typename InputIt>
auto entropy10(InputIt first, InputIt last)
{
    return entropy(first, last, [] (auto x) { return std::log10(x); });
}

template <typename Range>
auto entropy10(const Range& values)
{
    return entropy10(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename IntegerType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_binomial_coefficient(const IntegerType n, const IntegerType k)
{
    return log_factorial<RealType>(n) - (log_factorial<RealType>(k) + log_factorial<RealType>(n - k));
}

template <typename IntegerType, typename RealType = double,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_fisher_exact_test(const IntegerType a, const IntegerType b, const IntegerType c, const IntegerType d)
{
    return log_binomial_coefficient<RealType>(a + b, b) + log_binomial_coefficient<RealType>(c + d, d)
                - log_binomial_coefficient<RealType>(a + b + c + d, b + d);
}

template <typename IntegerType, typename RealType = double,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType fisher_exact_test(const IntegerType a, const IntegerType b, const IntegerType c, const IntegerType d)
{
    return std::exp(log_fisher_exact_test<IntegerType, RealType>(a, b, c, d));
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType geometric_pdf(const IntegerType k, const RealType p)
{
    boost::math::geometric_distribution<RealType> dist {p};
    return boost::math::pdf(dist, k);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType binomial_pdf(const IntegerType k, const IntegerType n, const RealType p)
{
    boost::math::binomial_distribution<RealType> dist {static_cast<RealType>(n), p};
    return boost::math::pdf(dist, k);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_poisson_pmf(const IntegerType k, const RealType mu)
{
    if (k > 0) {
        return k * std::log(mu) - std::lgamma(k) - mu;
    } else {
        return -mu;
    }
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType poisson_cdf(const IntegerType k, const RealType mu)
{
    return almost_zero(mu) ? 1.0 : boost::math::gamma_q(k + 1, mu);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType poisson_sf(const IntegerType k, const RealType mu)
{
    return almost_zero(mu) ? 0.0 : boost::math::gamma_p(k + 1, mu);
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_poisson_cdf(const IntegerType k, const RealType mu)
{
    return std::log(poisson_cdf(k, mu));
}

template <typename IntegerType, typename RealType,
          typename = std::enable_if_t<std::is_integral<IntegerType>::value>,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType log_poisson_sf(const IntegerType k, const RealType mu)
{
    return std::log(poisson_sf(k, mu));
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType normal_cdf(const RealType x, const RealType mu, const RealType sigma)
{
    boost::math::normal_distribution<RealType> dist {mu, sigma};
    return boost::math::cdf(dist, x);
}

template <typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
RealType normal_sf(const RealType x, const RealType mu, const RealType sigma)
{
    boost::math::normal_distribution<RealType> dist {mu, sigma};
    return boost::math::cdf(boost::math::complement(dist, x));
}

template <typename ForwardIt>
auto log_beta(const ForwardIt first, const ForwardIt last)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_beta is only defined for floating point types.");
    return std::accumulate(first, last, T {0}, [] (const auto curr, const auto x) { return curr + std::lgamma(x); })
           - std::lgamma(std::accumulate(first, last, T {0}));
}

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

template <typename ForwardIt1, typename ForwardIt2>
auto log_dirichlet(ForwardIt1 firstalpha, ForwardIt1 lastalpha, ForwardIt2 firstpi)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt1>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    return std::inner_product(firstalpha, lastalpha, firstpi, T {0}, std::plus<> {},
                              [] (const auto a, const auto p) { return (a - 1) * std::log(p); })
            - log_beta(firstalpha, lastalpha);
}

template <typename Container1, typename Container2>
auto log_dirichlet(const Container1& alpha, const Container2& pi)
{
    return log_dirichlet(std::cbegin(alpha), std::cend(alpha), std::cbegin(pi));
}

template <typename ForwardIt>
auto dirichlet_expectation(ForwardIt first_alpha, ForwardIt last_alpha)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    const auto K = static_cast<std::size_t>(std::distance(first_alpha, last_alpha));
    const auto a0 = std::accumulate(first_alpha, last_alpha, T {0});
    std::vector<T> result(K);
    std::transform(first_alpha, last_alpha, std::begin(result), [a0] (auto a) { return a / a0; });
    return result;
}

template <typename Range>
auto dirichlet_expectation(const Range& values)
{
    return dirichlet_expectation(std::cbegin(values), std::cend(values));
}

template <typename ForwardIt>
auto dirichlet_expectation(const unsigned i, ForwardIt first_alpha, ForwardIt last_alpha)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    assert(i < static_cast<unsigned>(std::distance(first_alpha, last_alpha)));
    return *std::next(first_alpha, i) / std::accumulate(first_alpha, last_alpha, T {0});
}

template <typename Range>
auto dirichlet_expectation(const unsigned i, const Range& values)
{
    return dirichlet_expectation(i, std::cbegin(values), std::cend(values));
}

template <typename ForwardIt>
auto dirichlet_entropy(ForwardIt first_alpha, ForwardIt last_alpha)
{
    using T = std::decay_t<typename std::iterator_traits<ForwardIt>::value_type>;
    static_assert(std::is_floating_point<T>::value,
                  "log_dirichlet is only defined for floating point types.");
    const auto K = static_cast<T>(std::distance(first_alpha, last_alpha));
    const auto a0 = std::accumulate(first_alpha, last_alpha, T {0});
    using boost::math::digamma;
    return log_beta(first_alpha, last_alpha) + (a0 - K) * digamma(a0)
           - std::accumulate(first_alpha, last_alpha, T {0}, [] (auto curr, auto a) { return curr + (a - 1) * digamma(a); });
}

template <typename Range>
auto dirichlet_entropy(const Range& values)
{
    return dirichlet_entropy(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename IntegerType>
inline RealType log_multinomial_coefficient(std::initializer_list<IntegerType> il)
{
    using std::begin; using std::end; using std::cbegin; using std::cend; using std::accumulate;
    std::vector<RealType> denoms(il.size());
    std::transform(cbegin(il), cend(il), begin(denoms), log_factorial<RealType, IntegerType>);
    return log_factorial<RealType>(accumulate(cbegin(il), cend(il), 0))
            - accumulate(cbegin(denoms), cend(denoms), RealType {0});
}

template <typename RealType, typename Iterator>
inline RealType log_multinomial_coefficient(Iterator first, Iterator last)
{
    using IntegerType = typename Iterator::value_type;
    std::vector<RealType> denoms(std::distance(first, last));
    std::transform(first, last, std::begin(denoms), log_factorial<RealType, IntegerType>);
    return log_factorial<RealType, IntegerType>(std::accumulate(first, last, IntegerType {0}))
            - std::accumulate(std::cbegin(denoms), std::cend(denoms), RealType {0});
}

template <typename RealType, typename Container>
inline RealType log_multinomial_coefficient(const Container& values)
{
    return log_multinomial_coefficient<RealType>(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename IntegerType>
inline IntegerType multinomial_coefficient(std::initializer_list<IntegerType> il)
{
    return static_cast<IntegerType>(std::exp(log_multinomial_coefficient<RealType, IntegerType>(std::move(il))));
}

template <typename IntegerType, typename RealType, typename Iterator>
inline IntegerType multinomial_coefficient(Iterator first, Iterator last)
{
    return static_cast<IntegerType>(std::exp(log_multinomial_coefficient<RealType>(first, last)));
}

template <typename IntegerType, typename RealType, typename Container>
inline IntegerType multinomial_coefficient(const Container& values)
{
    return multinomial_coefficient<IntegerType, RealType>(std::cbegin(values), std::cend(values));
}

template <typename RealType, typename ForwardIt1, typename ForwardIt2>
inline RealType multinomial_pdf(ForwardIt1 first_z, ForwardIt1 last_z, ForwardIt2 first_p)
{
    auto r = std::inner_product(first_z, last_z, first_p, RealType {0}, std::multiplies<> {},
                                [] (auto z_i, auto p_i) { return std::pow(p_i, z_i); });
    return multinomial_coefficient<RealType>(first_z, last_z) * r;
}

template <typename IntegerType, typename RealType>
inline RealType multinomial_pdf(const std::vector<IntegerType>& z, const std::vector<RealType>& p)
{
    assert(z.size() == p.size());
    return multinomial_pdf<RealType>(std::cbegin(z), std::cend(z), std::cbegin(p));
}

template <typename RealType, typename ForwardIt1, typename ForwardIt2>
inline RealType log_multinomial_pdf(ForwardIt1 first_z, ForwardIt1 last_z, ForwardIt2 first_p)
{
    auto r = std::inner_product(first_z, last_z, first_p, RealType {0}, std::plus<> {},
                                [] (auto z_i, auto p_i) { return z_i > 0 ? z_i * std::log(p_i) : 0.0; });
    return log_multinomial_coefficient<RealType>(first_z, last_z) + r;
}

template <typename IntegerType, typename RealType>
inline RealType log_multinomial_pdf(const std::vector<IntegerType>& z, const std::vector<RealType>& p)
{
    assert(z.size() == p.size());
    return log_multinomial_pdf<RealType>(std::cbegin(z), std::cend(z), std::cbegin(p));
}

// Returns approximate y such that digamma(y) = x
template <typename RealType>
inline RealType digamma_inv(const RealType x, const RealType epsilon = 10e-8)
{
    RealType l {1};
    auto y = std::exp(x);
    while (l > epsilon) {
        y += l * boost::math::sign(x - boost::math::digamma<RealType>(y));
        l /= 2;
    }
    return y;
}

namespace detail {

template <typename T, typename RealType>
T ifactorial(RealType x, std::true_type)
{
    return factorial<T, unsigned>(x);
}

template <typename T, typename RealType>
T ifactorial(RealType x, std::false_type)
{
    return factorial<T>(x);
}

template <typename T, typename RealType>
T ifactorial(RealType x)
{
    return ifactorial<T>(x, std::is_floating_point<RealType> {});
}

} // namespace detail

template <typename RealType>
RealType dirichlet_multinomial(const RealType z1, const RealType z2, const RealType a1, const RealType a2)
{
    auto z_0 = z1 + z2;
    auto a_0 = a1 + a2;
    using detail::ifactorial;
    auto z_m = ifactorial<RealType>(z1) * ifactorial<RealType>(z2);
    return (ifactorial<RealType>(z_0) / z_m) *
            (std::tgamma(a_0) / std::tgamma(z_0 + a_0)) *
            (std::tgamma(z1 + a1) * std::tgamma(z2 + a2)) / (std::tgamma(a1) + std::tgamma(a2));
}

template <typename RealType>
RealType dirichlet_multinomial(const RealType z1, const RealType z2, const RealType z3,
                               const RealType a1, const RealType a2, const RealType a3)
{
    auto z_0 = z1 + z2 + z3;
    auto a_0 = a1 + a2 + a3;
    using detail::ifactorial;
    auto z_m = ifactorial<RealType>(z1) * ifactorial<RealType>(z2) * ifactorial<RealType>(z3);
    return (ifactorial<RealType>(z_0) / z_m) *
            (std::tgamma(a_0) / std::tgamma(z_0 + a_0)) *
            (std::tgamma(z1 + a1) * std::tgamma(z2 + a2) *
            std::tgamma(z3 + a3)) / (std::tgamma(a1) + std::tgamma(a2) + std::tgamma(a3));
}

template <typename RealType>
RealType dirichlet_multinomial(const std::vector<RealType>& z, const std::vector<RealType>& a)
{
    auto z_0 = std::accumulate(std::cbegin(z), std::cend(z), RealType {0});
    auto a_0 = std::accumulate(std::cbegin(a), std::cend(a), RealType {0});
    RealType z_m {1};
    using detail::ifactorial;
    for (auto z_i : z) {
        z_m *= ifactorial<RealType>(z_i);
    }
    RealType g {1};
    for (std::size_t i {0}; i < z.size(); ++i) {
        g *= std::tgamma(z[i] + a[i]) / std::tgamma(a[i]);
    }
    return (ifactorial<RealType>(z_0) / z_m) * (std::tgamma(a_0) / std::tgamma(z_0 + a_0)) * g;
}

template <typename RealType>
RealType beta_binomial(const RealType k, const RealType n, const RealType alpha, const RealType beta)
{
    return dirichlet_multinomial<RealType>(k, n - k, alpha, beta);
}

namespace detail {

template <typename RealType>
bool is_mldp_converged(std::vector<RealType>& lhs, const std::vector<RealType>& rhs,
                       const RealType epsilon)
{
    std::transform(std::cbegin(lhs), std::cend(lhs), std::cbegin(rhs), std::begin(lhs),
                   [] (const auto a, const auto b) { return std::abs(a - b); });
    return std::all_of(std::cbegin(lhs), std::cend(lhs),
                       [epsilon] (const auto x) { return x < epsilon; });
}

} // namespace detail

template <typename RealType>
std::vector<RealType>
dirichlet_mle(std::vector<RealType> pi, const RealType precision,
              const unsigned max_iterations = 100, const RealType epsilon = 0.0001)
{
    std::transform(std::cbegin(pi), std::cend(pi), std::begin(pi),
                   [] (const auto p) { return std::log(p); });
    const auto l = pi.size();
    const RealType u {RealType {1} / l};
    std::vector<RealType> result(l, u), curr_result(l, u), means(l, u);
    for (unsigned n {0}; n < max_iterations; ++n) {
        RealType v {0};
        for (std::size_t j {0}; j < l; ++j) {
            v += means[j] * (pi[j] - boost::math::digamma<RealType>(precision * means[j]));
        }
        for (std::size_t k {0}; k < l; ++k) {
            curr_result[k] = digamma_inv<RealType>(pi[k] - v);
            means[k] = curr_result[k] / std::accumulate(std::cbegin(curr_result), std::cend(curr_result), RealType {0});
        }
        if (detail::is_mldp_converged(result, curr_result, epsilon)) {
            return curr_result;
        }
        result = curr_result;
    }
    return result;
}

template <typename NumericType = float, typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
NumericType probability_true_to_phred(const RealType p)
{
    return NumericType{-10} * std::log10(std::max(RealType {1} - p, std::numeric_limits<RealType>::epsilon()));
}

template <typename NumericType = float, typename RealType,
          typename = std::enable_if_t<std::is_floating_point<RealType>::value>>
NumericType probability_true_to_phred(const RealType p, const unsigned precision)
{
    return round(static_cast<NumericType>(RealType {-10} * std::log10(std::max(RealType {1} - p, std::numeric_limits<RealType>::epsilon()))), precision);
}

template <typename RealType = double, typename NumericType>
RealType phred_to_probability(const NumericType phred)
{
    return RealType {1} - std::pow(RealType {10}, RealType {-1} * static_cast<RealType>(phred) / RealType {10});
}

template <typename MapType>
typename MapType::key_type sum_keys(const MapType& map)
{
    return std::accumulate(std::cbegin(map), std::cend(map), typename MapType::key_type {},
                           [] (const auto previous, const auto& p) { return previous + p.first; });
}

template <typename ResultType, typename MapType, typename UnaryOperation>
ResultType sum_keys(const MapType& map, UnaryOperation op)
{
    return std::accumulate(std::cbegin(map), std::cend(map), ResultType {0},
                           [op] (const auto previous, const auto& p) { return previous + op(p.first); });
}

template <typename MapType>
typename MapType::mapped_type sum_values(const MapType& map)
{
    return std::accumulate(std::cbegin(map), std::cend(map), typename MapType::mapped_type {},
                           [] (const auto previous, const auto& p) { return previous + p.second; });
}

template <typename ResultType, typename MapType, typename UnaryOperation>
ResultType sum_values(const MapType& map, UnaryOperation op)
{
    return std::accumulate(std::cbegin(map), std::cend(map), ResultType {0},
                           [op] (const auto previous, const auto& p) { return previous + op(p.second); });
}

template <typename Map>
std::size_t sum_sizes(const Map& map)
{
    return std::accumulate(std::cbegin(map), std::cend(map), std::size_t {0},
                           [] (const auto& p, const auto& v) { return p + v.second.size(); });
}

template <typename InputIt1, typename InputIt2, typename InputIt3, typename T,
          typename BinaryOperation1, typename BinaryOperation2>
T inner_product(InputIt1 first1, InputIt1 last1,
                InputIt2 first2, InputIt3 first3, T value,
                BinaryOperation1 op1, BinaryOperation2 op2)
{
    while (first1 != last1) {
        value = op1(value, op2(*first1, *first2, *first3));
        ++first1;
        ++first2;
        ++first3;
    }
    return value;
}

template <typename InputIt1, typename InputIt2, typename InputIt3, typename InputIt4,
          typename T, typename BinaryOperation1, typename BinaryOperation2>
T inner_product(InputIt1 first1, InputIt1 last1,
                InputIt2 first2, InputIt3 first3,
                InputIt4 first4, T value,
                BinaryOperation1 op1, BinaryOperation2 op2)
{
    while (first1 != last1) {
        value = op1(value, op2(*first1, *first2, *first3, *first4));
        ++first1;
        ++first2;
        ++first3;
        ++first4;
    }
    return value;
}

template <typename RealType>
RealType beta_cdf(const RealType a, const RealType b, const RealType x)
{
    const boost::math::beta_distribution<> beta_dist {a, b};
    return boost::math::cdf(beta_dist, x);
}

template <typename RealType>
RealType beta_sf(const RealType a, const RealType b, const RealType x)
{
    const boost::math::beta_distribution<> beta_dist {a, b};
    return boost::math::cdf(boost::math::complement(beta_dist, x));
}

template <typename RealType>
RealType beta_tail_probability(const RealType a, const RealType b, const RealType x)
{
    return beta_cdf(a, b, x) + beta_sf(a, b, RealType {1} - x);
}

namespace detail {

template <typename RealType>
std::pair<RealType, RealType>
uniform_hdi(const RealType mass)
{
    const auto x = RealType {0.5} - mass / 2;
    return std::make_pair(x, x + mass);
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_symmetric(const RealType a, const RealType mass)
{
    const auto x = boost::math::ibeta_inv(a, a, (RealType {1} - mass) / 2);
    return std::make_pair(x, RealType {1} - x);
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_unbounded_rhs(const RealType a, const RealType mass)
{
    // Reverse J shaped
    return std::make_pair(boost::math::ibeta_inv(a, RealType {1}, RealType {1} - mass), RealType {1});
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_unbounded_lhs(const RealType b, const RealType mass)
{
    // J shaped
    return std::make_pair(RealType {0}, boost::math::ibeta_inv(RealType {1}, b, mass));
}

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi_skewed(const RealType a, const RealType b, const RealType mass)
{
    const auto c = (RealType {1} - mass) / 2;
    return std::make_pair(boost::math::ibeta_inv(a, b, c), boost::math::ibeta_inv(a, b, c + mass));
}

} // namespace detail

template <typename RealType>
std::pair<RealType, RealType>
beta_hdi(RealType a, RealType b, const RealType mass)
{
    static_assert(std::is_floating_point<RealType>::value, "beta_hdi only works for floating point types");
    if (mass < RealType {0} || mass > RealType {1}) {
        throw std::domain_error {"beta_hdi: given mass not in range [0, 1]"};
    }
    if (a <= RealType {0} || b <= RealType {0}) {
        throw std::domain_error {"beta_hdi: given non-positive parameter"};
    }
    if (mass == RealType {0}) {
        const auto mean = a / (a + b);
        return std::make_pair(mean, mean);
    }
    if (mass == RealType {1}) {
        return std::make_pair(RealType {0}, RealType {1});
    }
    if (a == b) {
        if (a == RealType {1}) {
            return detail::uniform_hdi(mass);
        } else {
            return detail::beta_hdi_symmetric(a, mass);
        }
    }
    if (a == RealType {1}) {
        return detail::beta_hdi_unbounded_lhs(b, mass);
    }
    if (b == RealType {1}) {
        return detail::beta_hdi_unbounded_rhs(a, mass);
    }
    return detail::beta_hdi_skewed(a, b, mass);
}

template <typename RealType>
RealType dirichlet_variance(const std::vector<RealType>& alphas, const std::size_t k)
{
    const auto a_0 = std::accumulate(std::cbegin(alphas), std::cend(alphas), RealType {});
    return (alphas[k] * (a_0 - alphas[k])) / (a_0 * a_0 * (a_0 + 1));
}

template <typename RealType>
RealType dirichlet_marginal_cdf(const std::vector<RealType>& alphas, const std::size_t k, const RealType x)
{
    const auto a_0 = std::accumulate(std::cbegin(alphas), std::cend(alphas), RealType {});
    return beta_cdf(alphas[k], a_0 - alphas[k], x);
}

template <typename RealType>
RealType dirichlet_marginal_sf(const std::vector<RealType>& alphas, const std::size_t k, const RealType x)
{
    const auto a_0 = std::accumulate(std::cbegin(alphas), std::cend(alphas), RealType {});
    return beta_sf(alphas[k], a_0 - alphas[k], x);
}

template <typename Range>
void log_each(Range& values)
{
    for (auto& v : values) v = std::log(v);
}

template <typename Range>
void exp_each(Range& values)
{
    for (auto& v : values) v = std::exp(v);
}

template <typename ForwardIterator>
auto normalise(const ForwardIterator first, const ForwardIterator last)
{
    using T = typename std::iterator_traits<ForwardIterator>::value_type;
    const auto norm = std::accumulate(first, last, T {});
    if (norm > 0) std::for_each(first, last, [norm] (auto& value) { value /= norm; });
    return norm;
}

template <typename Range>
auto normalise(Range& values)
{
    return normalise(std::begin(values), std::end(values));
}

template <typename ForwardIterator>
auto normalise_logs(const ForwardIterator first, const ForwardIterator last)
{
    const auto norm = log_sum_exp(first, last);
    std::for_each(first, last, [norm] (auto& value) { value -= norm; });
    return norm;
}

template <typename Range>
auto normalise_logs(Range& logs)
{
    return normalise_logs(std::begin(logs), std::end(logs));
}

template <typename ForwardIterator>
auto normalise_exp(const ForwardIterator first, const ForwardIterator last)
{
    const auto norm = log_sum_exp(first, last);
    std::transform(first, last, first, [norm] (auto& value) { return std::exp(value - norm); });
    return norm;
}

template <typename Range>
auto normalise_exp(Range& logs)
{
    return normalise_exp(std::begin(logs), std::end(logs));
}

} // namespace maths
} // namespace octopus

#endif


#include <iostream>
#include <ctime>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/numeric/ublas/vector.hpp>
#include "boost/program_options.hpp"
#include "boost/random.hpp"

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

typedef matrix<double, column_major> uBlasMat;
typedef vector<double, column_major> uBlasVec;
typedef boost::minstd_rand base_generator_type;
typedef boost::variate_generator<base_generator_type&, boost::uniform_real<> > rng;

uBlasMat initMatrix(uBlasMat &mat, rng &rnd) {

  for (unsigned i = 0; i < mat.size1(); ++i) {
    for (unsigned j = 0; j < mat.size2(); ++j) {
      mat (i, j) = rnd();
    }
  }

  return mat;
}

uBlasVec initVector(uBlasVec &vec, rng &rnd) {

  for (unsigned i = 0; i < mat.size(); ++i) {
      vec (i) = rnd();
  }

  return vec;
}

// m: numRows, n: numCols
inline double simpleDenseTest_UBlas(int m, int n, int num_trials) {
  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);

  uBlasMat A(m, n); initMatrix(A, uni);
  uBlasMat B(m, n); initMatrix(B, uni);
  uBlasMat C(m, n); initMatrix(C, uni);
  uBlasMat D(m, n); initMatrix(D, uni);
  uBlasMat E(m, n); initMatrix(E, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    uBlasMat res = element_prod((element_div((A + B), C) - D), E);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;

}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmSanityTest_UBlas(int m, int n, int k, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasMat A(m, n); initMatrix(A, uni);
  uBlasMat C(n, k); initMatrix(C, uni);
  uBlasMat E(m, k); initMatrix(E, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E += prod(A, C);

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double gemmDenseTest_UBlas(int m, int n, int k, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasMat A(m, n); initMatrix(A, uni);
  uBlasMat B(m, n); initMatrix(B, uni);
  uBlasMat C(n, k); initMatrix(C, uni);
  uBlasMat D(n, k); initMatrix(D, uni);
  uBlasMat E(m, k); initMatrix(E, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    E += prod((A + B), (C - D));

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

inline double mulDenseTest_UBlas(int a, int b, int c, int d, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasMat A(a, a); initMatrix(A, uni);
  uBlasMat B(a, b); initMatrix(B, uni);
  uBlasMat C(b, c); initMatrix(C, uni);
  uBlasMat D(c, d); initMatrix(D, uni);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    uBlasMat res = uBlasMat(prod(uBlasMat(prod(uBlasMat(prod(A, B)), C)), D));

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;
  return duration / num_trials;
}

// m: numRows of A, n: numCols of A, and numRows of B, k: numCols of B
inline double denseVectorTest_UBlas(int l, int num_trials) {

  base_generator_type generator(42u);
  boost::uniform_real<> uni_dist(0,1);
  rng uni(generator, uni_dist);
  uBlasVec a(l); initVector(a, uni);
  uBlasVec b(l); initVector(b, uni);
  uBlasVec c(l); initVector(c, uni);
  uBlasVec d(l); initVector(d, uni);
  uBlasVec res(l);

  clock_t start;
  start = clock();
  for (unsigned i = 0; i < num_trials; i++) {

    res = a + b + c + d;

  }
  double duration = ( std::clock() - start ) / (double) CLOCKS_PER_SEC;

  return duration / num_trials;
}

void runUBlasTests(int num_trials, int l, int m, int n, int k, int a, int b, int c, int d,
    bool skip_vec, bool skip_simple, bool skip_gemm, bool skip_mult) {

  if (!skip_vec)
      cout << "UBlas Vectors Test:\t" << denseVectorTest_UBlas(l, num_trials) << endl;
  if (!skip_simple)
    cout << "UBlas Simple Test:\t" << simpleDenseTest_UBlas(m, n, num_trials) << endl;
  if (!skip_gemm) {
    cout << "UBlas gemmSanity Test:\t" << gemmDenseTest_UBlas(m, n, k, num_trials) << endl;
    cout << "UBlas gemm Test:\t" << gemmDenseTest_UBlas(m, n, k, num_trials) << endl;
  }

  if (!skip_mult)
    cout << "UBlas mulDense Test:\t" << mulDenseTest_UBlas(a, b, c, d, num_trials) << endl;

}

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

    int l, m, n, k, a, b, c, d, trials;
    bool skip_vec, skip_simple, skip_gemm, skip_mult;

    po::options_description desc("Allowed options");
    desc.add_options()
        ("help", "produce help message")
        ("l", po::value<int>(&l)->default_value(1048576),
                    "length of vectors in vector addition test")
        ("m", po::value<int>(&m)->default_value(1024),
            "numRows of matrices in Simple Test, and gemm Test")
        ("n", po::value<int>(&n)->default_value(1024),
            "numCols of matrices in Simple Test, and gemm Test")
        ("k", po::value<int>(&k)->default_value(1024),
            "numCols of B in gemm Test")
        ("trials", po::value<int>(&trials)->default_value(10), "number of trials")
        ("a", po::value<int>(&a)->default_value(1024),
            "size matrix A in mulDense Test")
        ("b", po::value<int>(&b)->default_value(512),
            "size matrix B in mulDense Test")
        ("c", po::value<int>(&c)->default_value(256),
            "size matrix C in mulDense Test")
        ("d", po::value<int>(&d)->default_value(128),
            "size matrix D in mulDense Test")
        ("skip-vec", po::value<bool>(&skip_vec)->default_value(false),
            "skip vectors Test")
        ("skip-simple", po::value<bool>(&skip_simple)->default_value(false),
            "skip simple Test")
        ("skip-gemm", po::value<bool>(&skip_gemm)->default_value(false),
            "skip gemm Tests")
        ("skip-mult", po::value<bool>(&skip_mult)->default_value(false),
            "skip mulDense Test")
    ;

    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    runUBlasTests(trials, l, m, n, k, a, b, c, d, skip_vec, skip_simple, skip_gemm, skip_mult);

    return 0;
}


#include <iostream>
#include <boost/numeric/ublas/matrix.hpp>
#include <boost/numeric/ublas/matrix_proxy.hpp>
#include <boost/numeric/ublas/io.hpp>
#include <boost/algorithm/minmax.hpp>
#include <vector>
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>
#include <iterator>
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "dbscan.h"

using namespace std;
namespace clustering {
    DBSCAN::ClusterData DBSCAN::gen_cluster_data( size_t features_num, size_t elements_num ,double* data)//load data for clustering
{
    DBSCAN::ClusterData cl_d( elements_num, features_num );

    for ( size_t i = 0; i < elements_num; ++i )
    {
        for ( size_t j = 0; j < features_num; ++j )
        {
            cl_d( i, j ) = data[i*features_num+j];
        }
    }

    return cl_d;
}

DBSCAN::FeaturesWeights DBSCAN::std_weights( size_t s )
{
    // num cols
    DBSCAN::FeaturesWeights ws( s );

    for ( size_t i = 0; i < s; ++i ) {
        ws( i ) = 1.0;
    }

    return ws;
}

DBSCAN::DBSCAN()
{
}

static int num_threads_or_default( int nt )
{
    if ( !nt ) {
        return 0;//omp_get_max_threads();
    }
    return nt;
}

void DBSCAN::init( double eps, size_t min_elems, int num_threads )
{
    m_eps = eps;
    m_min_elems = min_elems;
    m_num_threads = num_threads_or_default( num_threads );
}

DBSCAN::DBSCAN( double eps, size_t min_elems, int num_threads )
    : m_eps( eps )
    , m_min_elems( min_elems )
    , m_num_threads( num_threads_or_default( num_threads ) )
    , m_dmin( 0.0 )
    , m_dmax( 0.0 )
{
    reset();
}

DBSCAN::~DBSCAN()
{
}

void DBSCAN::reset()
{
    m_labels.clear();
}

void DBSCAN::prepare_labels( size_t s )
{
    m_labels.resize( s );

    for ( auto& l : m_labels ) {
        l = -1;
    }
}

const DBSCAN::DistanceMatrix DBSCAN::calc_dist_matrix( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    DBSCAN::ClusterData cl_d = C;

 //   omp_set_dynamic( 0 );
 //   omp_set_num_threads( m_num_threads );
//#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size2(); ++i ) {
        ublas::matrix_column< DBSCAN::ClusterData > col( cl_d, i );

        const auto r = minmax_element( col.begin(), col.end() );

        double data_min = *r.first;
        double data_range = *r.second - *r.first;

        if ( data_range == 0.0 ) {
            data_range = 1.0;
        }

        const double scale = 1 / data_range;
        const double min = -1.0 * data_min * scale;

        col *= scale;
        col.plus_assign( ublas::scalar_vector< typename ublas::matrix_column< DBSCAN::ClusterData >::value_type >( col.size(), min ) );
    }

    // rows x rows
    DBSCAN::DistanceMatrix d_m( cl_d.size1(), cl_d.size1() );
    ublas::vector< double > d_max( cl_d.size1() );
    ublas::vector< double > d_min( cl_d.size1() );

 //   omp_set_dynamic( 0 );
 //   omp_set_num_threads( m_num_threads );
//#pragma omp parallel for
    for ( size_t i = 0; i < cl_d.size1(); ++i ) {
        for ( size_t j = i; j < cl_d.size1(); ++j ) {
            d_m( i, j ) = 0.0;

            if ( i != j ) {
                ublas::matrix_row< DBSCAN::ClusterData > U( cl_d, i );
                ublas::matrix_row< DBSCAN::ClusterData > V( cl_d, j );

                int k = 0;
                for ( const auto e : ( U - V ) ) {
                    d_m( i, j ) += fabs( e ) * W[k++];
                }

                d_m( j, i ) = d_m( i, j );
            }
        }

        const auto cur_row = ublas::matrix_row< DBSCAN::DistanceMatrix >( d_m, i );
        const auto mm = minmax_element( cur_row.begin(), cur_row.end() );

        d_max( i ) = *mm.second;
        d_min( i ) = *mm.first;
    }

    m_dmin = *( min_element( d_min.begin(), d_min.end() ) );
    m_dmax = *( max_element( d_max.begin(), d_max.end() ) );

    m_eps = ( m_dmax - m_dmin ) * m_eps + m_dmin;

    return d_m;
}

DBSCAN::Neighbors DBSCAN::find_neighbors( const DBSCAN::DistanceMatrix& D, uint32_t pid )
{
    Neighbors ne;

    for ( uint32_t j = 0; j < D.size1(); ++j ) {
        if ( D( pid, j ) <= m_eps ) {
            ne.push_back( j );
        }
    }
    return ne;
}

void DBSCAN::dbscan( const DBSCAN::DistanceMatrix& dm )
{
    std::vector< uint8_t > visited( dm.size1() );

    uint32_t cluster_id = 0;

    for ( uint32_t pid = 0; pid < dm.size1(); ++pid ) {
        if ( !visited[pid] ) {
            visited[pid] = 1;

            Neighbors ne = find_neighbors( dm, pid );

            if ( ne.size() >= m_min_elems ) {
                m_labels[pid] = cluster_id;

                for ( uint32_t i = 0; i < ne.size(); ++i ) {
                    uint32_t nPid = ne[i];

                    if ( !visited[nPid] ) {
                        visited[nPid] = 1;

                        Neighbors ne1 = find_neighbors( dm, nPid );

                        if ( ne1.size() >= m_min_elems ) {
                            for ( const auto& n1 : ne1 ) {
                                ne.push_back( n1 );
                            }
                        }
                    }

                    if ( m_labels[nPid] == -1 ) {
                        m_labels[nPid] = cluster_id;
                    }
                }

                ++cluster_id;
            }
        }
    }
}

void DBSCAN::fit( const DBSCAN::ClusterData& C )
{
    const DBSCAN::FeaturesWeights W = DBSCAN::std_weights( C.size2() );
    wfit( C, W );
}
void DBSCAN::fit_precomputed( const DBSCAN::DistanceMatrix& D )
{
    prepare_labels( D.size1() );
    dbscan( D );
}

void DBSCAN::wfit( const DBSCAN::ClusterData& C, const DBSCAN::FeaturesWeights& W )
{
    prepare_labels( C.size1() );
    const DBSCAN::DistanceMatrix D = calc_dist_matrix( C, W );
    dbscan( D );
}

const DBSCAN::Labels& DBSCAN::get_labels() const
{
    return m_labels;
}

    //save clusters
int DBSCAN::save_labels(std::string name, std::vector <std::string> ndata, std::vector <std::string> uid,std::vector <int>* ind, double * data)
{
    std::vector <std::vector <int>> final;
    std::vector <int> tmp;

    int mm=0;
    for (int j=0;j<m_labels.size();j++)
    {
        if((mm)<m_labels[j])
            mm=m_labels[j];
        ind->push_back(m_labels[j]);
    }

    //assign a cluster to each unassigned one I need Y to claculate the centres and then

    for (int j=0;j<=mm;j++)
    {
        final.push_back(tmp);
    }

    std::ofstream myfile (name);

  //  std::vector <double> cen;
  //  std::vector <int> ss;

/*    for(int i=0;i<=2*mm+1;i++)
    {
        cen.push_back(0);
        ss.push_back(0);
    }
    for (int i=0;i<ind->size();i++)
    {
        if(ind->at(i)>=0)
        {
            cen[ind->at(i)*2+1]=data[i*2+1];
            cen[ind->at(i)*2]=data[i*2];
            ss[ind->at(i)*2+1]++;
            ss[ind->at(i)*2]++;
        }
    }

    for (int i=0;i<cen.size();i++)
        cen[i]/=ss[i];

    ss.clear();*/
    for (int j=0;j<ind->size();j++)
    {
        if(ind->at(j)>=0)
            final[ind->at(j)].push_back(j);
      /*  else
        {
            double mind=1000000;
            int mini=-1;
            for (int i=0;i<=mm;i++)
            {
                double dist=sqrt((data[j*2+1]-cen[i*2+1])*(data[j*2+1]-cen[i*2+1])+(data[j*2]-cen[i*2])*(data[j*2]-cen[i*2]));
                if(dist<mind)
                {
                    mind=dist;
                    mini=i;
                }
            }
            ind->at(j)=mini;
            if(mini>=0)
                final[ind->at(j)].push_back(j);
        }*/
        myfile<<ind->at(j)<<", ";
    }
    myfile.close();

 //   cen.clear();
    for (int j=0;j<final.size();j++)
    {
        tmp.clear();
        copy((final[j]).begin(),final[j].end(),back_inserter(tmp));
        stringstream ss;
        ss << j;
        string str = ss.str();
        std::string name1=name+str;//static_cast<ostringstream*>( &(ostringstream() << j) )->str();
        std::ofstream myfile (name1);
        for (int k=0;k<tmp.size();k++)
        {
            myfile<<'>'<< uid[tmp[k]]<<"\n";
            myfile<<  ndata[tmp[k]]<<"\n";
        }
        myfile.close();
    }
    final.clear();
    tmp.clear();
    return mm;
}

std::ostream& operator<<( std::ostream& o, DBSCAN& d )
{
    o << "[ ";
    for ( const auto& l : d.get_labels() ) {
        o << " " << l;
    }
    o << " ] " << std::endl;

    return o;
}
}


#pragma once
#include <pdqsort.h>

#include <Eigen/Core>
#include <numeric>

namespace cpz {
namespace {
  template <typename Derived> bool is_regular(const Eigen::MatrixBase<Derived>& exponents) {
    const unsigned int num_cols = exponents.cols();
    for (unsigned int i = 0; i < num_cols - 1; ++i) {
      const auto& col = exponents.col(i);
      if (col.isZero()) {
        return false;
      }

      if (col == exponents.col(i + 1)) {
        return false;
      }
    }

    return true;
  }

  template <typename Derived>
  void permute_cols(Eigen::MatrixBase<Derived>& mat, std::vector<int>& permutation) {
    const unsigned int num_cols = mat.cols();
    unsigned int idx            = 0;
    int swap_start              = -1;
    unsigned int count          = 0;
    int permutation_idx         = -1;
    while (count < num_cols) {
      // Find the start point of the next chain of swaps
      ++swap_start;
      while (swap_start < num_cols && permutation[swap_start] < 0) {
        ++swap_start;
      }

      // Follow the chain of swaps
      idx             = swap_start;
      permutation_idx = permutation[swap_start];
      ++count;
      while (permutation_idx != swap_start) {
        mat.col(idx).swap(mat.col(permutation_idx));
        permutation[idx] = -1;
        idx              = permutation_idx;
        permutation_idx  = permutation[permutation_idx];
        ++count;
      }

      permutation[idx] = -1;
    }
  }

  template <typename Derived> auto unique_columns(const Eigen::MatrixBase<Derived>& exponents) {
    std::vector<std::vector<unsigned int>> column_groups;
    std::vector<unsigned int> unique_indices = {0};
    int curr_col                             = 0;
    std::vector<unsigned int> curr_group     = {0};
    const unsigned int num_cols              = exponents.cols();
    for (unsigned int i = 1; i < num_cols; ++i) {
      if (exponents.col(curr_col) != exponents.col(i)) {
        column_groups.push_back(curr_group);
        unique_indices.push_back(i);
        curr_group.clear();
        curr_col = i;
      }

      curr_group.push_back(i);
    }

    column_groups.push_back(curr_group);
    return std::make_pair(column_groups, exponents(Eigen::all, unique_indices));
  }

  template <typename D1, typename D2>
  auto
  regularize(const Eigen::MatrixBase<D1>& exponents, const Eigen::MatrixBase<D2>& generators) {
    const auto [column_groups, new_exponents] = unique_columns(exponents);
    const unsigned int num_groups             = column_groups.size();
    D2 new_generators(generators.rows(), num_groups);
    for (unsigned int i = 0; i < num_groups; ++i) {
      new_generators.col(i).noalias() = generators(Eigen::all, column_groups[i]).rowwise().sum();
    }

    return std::make_pair(new_exponents, new_generators);
  }

  template <typename D1, typename D2>
  void ensure_regular(Eigen::MatrixBase<D1>& exponents, Eigen::MatrixBase<D2>& generators) {
    // First, sort the exponents and generators according to the exponents
    const unsigned int num_cols = exponents.cols();
    const unsigned int num_rows = exponents.rows();
    if (num_cols == 0 || num_rows == 0) {
      return;
    }

    std::vector<int> permutation(num_cols);
    std::iota(permutation.begin(), permutation.end(), 0);
    pdqsort(permutation.begin(),
            permutation.end(),
            [&exponents, num_rows](const int i, const int j) -> bool {
              const auto col_a = exponents.col(i);
              const auto col_b = exponents.col(j);
              for (unsigned int i = 0; i < num_rows; ++i) {
                const auto val_a = col_a[i];
                const auto val_b = col_b[i];
                if (val_a != val_b) {
                  return val_a < val_b;
                }
              }

              return false;
            });

    // Then, apply the permutation in-place to sort the matrices
    // Because permute_cols modifies the permutation vector, we make a copy
    std::vector<int> permutation_copy(permutation);
    permute_cols(exponents, permutation_copy);
    permute_cols(generators, permutation);

    // Finally, check if the exponents matrix is regular and apply regularization if it is not
    if (!is_regular(exponents)) {
      std::tie(exponents, generators) = regularize(exponents, generators);
    }
  }
}  // namespace

template <typename F                  = float,
          int Dims                    = Eigen::Dynamic,
          int NumGenerators           = Eigen::Dynamic,
          int NumFactors              = Eigen::Dynamic,
          int NumConstraints          = Eigen::Dynamic,
          int NumConstraintGenerators = Eigen::Dynamic>
struct ConstrainedPolynomialZonotope {
 protected:
  inline void regularize_cpz() noexcept {
    ensure_regular(this->exponents, this->generators);
    ensure_regular(this->constraint_exponents, this->constraint_generators);
  }

  template <int Size> using Vector     = Eigen::Matrix<F, Size, 1>;
  template <int R, int C> using Matrix = Eigen::Matrix<F, R, C>;

 public:
  Vector<Dims> center;
  Matrix<Dims, NumGenerators> generators;
  Matrix<NumFactors, NumGenerators> exponents;
  Vector<NumConstraints> constraints;
  Matrix<NumConstraints, NumConstraintGenerators> constraint_generators;
  Matrix<NumFactors, NumConstraintGenerators> constraint_exponents;

  ConstrainedPolynomialZonotope(
  const Vector<Dims>& center,
  const Matrix<Dims, NumGenerators>& generators,
  const Matrix<NumFactors, NumGenerators>& exponents,
  const Vector<NumConstraints>& constraints,
  const Matrix<NumConstraints, NumConstraintGenerators>& constraint_generators,
  const Matrix<NumFactors, NumConstraintGenerators>& constraint_exponents)
  : center(center)
  , generators(generators)
  , exponents(exponents)
  , constraints(constraints)
  , constraint_generators(constraint_generators)
  , constraint_exponents(constraint_exponents) {
    regularize_cpz();
  }

  ConstrainedPolynomialZonotope(
  const Vector<Dims>&& center,
  const Matrix<Dims, NumGenerators>&& generators,
  const Matrix<NumFactors, NumGenerators>&& exponents,
  const Vector<NumConstraints>&& constraints,
  const Matrix<NumConstraints, NumConstraintGenerators>&& constraint_generators,
  const Matrix<NumFactors, NumConstraintGenerators>&& constraint_exponents)
  : center(center)
  , generators(generators)
  , exponents(exponents)
  , constraints(constraints)
  , constraint_generators(constraint_generators)
  , constraint_exponents(constraint_exponents) {
    regularize_cpz();
  }
};
}  // namespace cpz


//---------------------------------------------------------------------------//
// Copyright (c) 2013-2014 Mageswaran.D <mageswaran1989@gmail.com>
//
// 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
//
// See http://boostorg.github.com/compute for more information.
//---------------------------------------------------------------------------//

#include <iostream>
#include <string>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include <boost/compute/system.hpp>
#include <boost/compute/interop/opencv/core.hpp>
#include <boost/compute/interop/opencv/highgui.hpp>
#include <boost/compute/utility/source.hpp>

#include <boost/program_options.hpp>

namespace compute = boost::compute;
namespace po = boost::program_options;

// Create naive optical flow program
const char source[] = BOOST_COMPUTE_STRINGIZE_SOURCE (
    const sampler_t sampler = CLK_ADDRESS_CLAMP_TO_EDGE;

    __kernel void optical_flow (
                                read_only
                                image2d_t current_image,
                                read_only image2d_t previous_image,
                                write_only image2d_t optical_flow,
                                const float scale,
                                const float offset,
                                const float lambda,
                                const float threshold )
    {
        int2 coords = (int2)(get_global_id(0), get_global_id(1));
        float4 current_pixel    = read_imagef(current_image,
                                              sampler,
                                              coords);
        float4 previous_pixel   = read_imagef(previous_image,
                                              sampler,
                                              coords);
        int2 x1     = (int2)(offset, 0.f);
        int2 y1     = (int2)(0.f, offset);

        //get the difference
        float4 curdif = previous_pixel - current_pixel;

        //calculate the gradient
        //Image 2 first
        float4 gradx = read_imagef(previous_image,
                                   sampler,
                                   coords+x1) -
                       read_imagef(previous_image,
                                   sampler,
                                   coords-x1);
        //Image 1
        gradx += read_imagef(current_image,
                             sampler,
                             coords+x1) -
                 read_imagef(current_image,
                             sampler,
                             coords-x1);
        //Image 2 first
        float4 grady = read_imagef(previous_image,
                                   sampler,
                                   coords+y1) -
                       read_imagef(previous_image,
                                   sampler,
                                   coords-y1);
        //Image 1
        grady += read_imagef(current_image,
                             sampler,
                             coords+y1) -
                 read_imagef(current_image,
                             sampler,
                             coords-y1);

        float4 sqr = (gradx*gradx) + (grady*grady) +
                     (float4)(lambda,lambda, lambda, lambda);
        float4 gradmag = sqrt(sqr);

        ///////////////////////////////////////////////////
        float4 vx = curdif * (gradx / gradmag);
        float vxd = vx.x;//assumes greyscale

        //format output for flowrepos, out(-x,+x,-y,+y)
        float2 xout = (float2)(fmax(vxd,0.f),fabs(fmin(vxd,0.f)));
        xout *= scale;
        ///////////////////////////////////////////////////
        float4 vy = curdif*(grady/gradmag);
        float vyd = vy.x;//assumes greyscale

        //format output for flowrepos, out(-x,+x,-y,+y)
        float2 yout = (float2)(fmax(vyd,0.f),fabs(fmin(vyd,0.f)));
        yout *= scale;
        ///////////////////////////////////////////////////

        float4 out = (float4)(xout, yout);
        float cond = (float)isgreaterequal(length(out), threshold);
        out *= cond;

        write_imagef(optical_flow, coords, out);
    }
);

// This example shows how to read two images or use camera
// with OpenCV, transfer the frames to the GPU,
// and apply a naive optical flow algorithm
// written in OpenCL
int main(int argc, char *argv[])
{
    // setup the command line arguments
    po::options_description desc;
    desc.add_options()
            ("help",  "show available options")
            ("camera", po::value<int>()->default_value(-1),
                                 "if not default camera, specify a camera id")
            ("image1", po::value<std::string>(), "path to image file 1")
            ("image2", po::value<std::string>(), "path to image file 2");

    // Parse the command lines
    po::variables_map vm;
    po::store(po::parse_command_line(argc, argv, desc), vm);
    po::notify(vm);

    //check the command line arguments
    if(vm.count("help"))
    {
        std::cout << desc << std::endl;
        return 0;
    }

    //OpenCV variables
    cv::Mat previous_cv_image;
    cv::Mat current_cv_image;
    cv::VideoCapture cap; //OpenCV camera handle

    //check for image paths
    if(vm.count("image1") && vm.count("image2"))
    {
        // Read image 1 with OpenCV
        previous_cv_image = cv::imread(vm["image1"].as<std::string>(),
                                       CV_LOAD_IMAGE_COLOR);
        if(!previous_cv_image.data){
            std::cerr << "Failed to load image" << std::endl;
            return -1;
        }

        // Read image 2 with opencv
        current_cv_image = cv::imread(vm["image2"].as<std::string>(),
                                      CV_LOAD_IMAGE_COLOR);
        if(!current_cv_image.data){
            std::cerr << "Failed to load image" << std::endl;
            return -1;
        }
    }
    else //by default use camera
    {
        //open camera
        cap.open(vm["camera"].as<int>());
        // read first frame
        cap >> previous_cv_image;
        if(!previous_cv_image.data){
            std::cerr << "failed to capture frame" << std::endl;
            return -1;
        }

        // read second frame
        cap >> current_cv_image;
        if(!current_cv_image.data){
            std::cerr << "failed to capture frame" << std::endl;
            return -1;
        }

    }

    // Get default device and setup context
    compute::device gpu = compute::system::default_device();
    compute::context context(gpu);
    compute::command_queue queue(context, gpu);

    // Convert image to BGRA (OpenCL requires 16-byte aligned data)
    cv::cvtColor(previous_cv_image, previous_cv_image, CV_BGR2BGRA);
    cv::cvtColor(current_cv_image, current_cv_image, CV_BGR2BGRA);

    // Transfer image to gpu
    compute::image2d dev_previous_image =
            compute::opencv_create_image2d_with_mat(
                previous_cv_image, compute::image2d::read_write, queue
                );
    // Transfer image to gpu
    compute::image2d dev_current_image =
            compute::opencv_create_image2d_with_mat(
                current_cv_image, compute::image2d::read_write, queue
                );

    // Create output image
    compute::image2d dev_output_image(
                context,
                dev_previous_image.width(),
                dev_previous_image.height(),
                dev_previous_image.format(),
                compute::image2d::write_only
                );

    compute::program optical_program =
            compute::program::create_with_source(source, context);
    optical_program.build();

    // create flip kernel and set arguments
    compute::kernel optical_kernel(optical_program, "optical_flow");
    float scale = 10;
    float offset = 1;
    float lambda = 0.0025;
    float threshold = 1.0;

    optical_kernel.set_arg(0, dev_previous_image);
    optical_kernel.set_arg(1, dev_current_image);
    optical_kernel.set_arg(2, dev_output_image);
    optical_kernel.set_arg(3, scale);
    optical_kernel.set_arg(4, offset);
    optical_kernel.set_arg(5, lambda);
    optical_kernel.set_arg(6, threshold);

    // run flip kernel
    size_t origin[2] = { 0, 0 };
    size_t region[2] = { dev_previous_image.width(),
                         dev_previous_image.height() };
    queue.enqueue_nd_range_kernel(optical_kernel, 2, origin, region, 0);

    //check for image paths
    if(vm.count("image1") && vm.count("image2"))
    {
        // show host image
        cv::imshow("Previous Frame", previous_cv_image);
        cv::imshow("Current Frame", current_cv_image);

        // show gpu image
        compute::opencv_imshow("filtered image", dev_output_image, queue);

        // wait and return
        cv::waitKey(0);
    }
    else
    {
        char key = '\0';
        while(key != 27) //check for escape key
        {
            cap >> current_cv_image;

            // Convert image to BGRA (OpenCL requires 16-byte aligned data)
            cv::cvtColor(current_cv_image, current_cv_image, CV_BGR2BGRA);

            // Update the device image memory with current frame data
            compute::opencv_copy_mat_to_image(previous_cv_image,
                                              dev_previous_image,
                                              queue);
            compute::opencv_copy_mat_to_image(current_cv_image,
                                              dev_current_image,
                                              queue);

            // Run the kernel on the device
            queue.enqueue_nd_range_kernel(optical_kernel, 2, origin, region, 0);

            // Show host image
            cv::imshow("Previous Frame", previous_cv_image);
            cv::imshow("Current Frame", current_cv_image);

            // Show GPU image
            compute::opencv_imshow("filtered image", dev_output_image, queue);

            // Copy current frame container to previous frame container
            current_cv_image.copyTo(previous_cv_image);

            // wait
            key = cv::waitKey(10);
        }

    }
    return 0;
}


/*! ------------------------------------------------------------------------- *
 * \author Joey Dumont                  <joey.dumont@gmail.com>               *
 * \since 2018-07-24                                                          *
 *                                                                            *
 * Outputs a 10x10 matrix with random complex numbers in a human-readable     *
 * format.                                                                    *
 * --------------------------------------------------------------------------*/

#include <armadillo>

int main(int argc, char* argv[])
{
  arma::cx_mat rand_cx_mat = arma::randu<arma::cx_mat>(10,10);
  rand_cx_mat.save("rand_test.txt", arma::raw_ascii);
  return 0;
}