// SPDX-License-Identifier: MIT
// SPDX-FileCopyrightText: Copyright 2019-2021 Heal Research

#define DOCTEST_CONFIG_IMPLEMENT_WITH_MAIN
#include <doctest/doctest.h>

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics.hpp>

#define ANKERL_NANOBENCH_IMPLEMENT
#include "nanobench.h"

#include <iostream>
#include <memory>
#include <random>
#include <vector>

#include "gsl/gsl_statistics_double.h"
#include "gsl/gsl_statistics_float.h"

#include <chrono>

#include "vstat/vstat.hpp"

#include "Statistics.h"

namespace ba = boost::accumulators;
namespace nb = ankerl::nanobench;

struct Foo {
    double value;
};

TEST_SUITE("usage")
{
    TEST_CASE("univariate")
    {
        std::vector<float> values { 1.0, 2.0, 3.0, 4.0 };
        std::vector<float> weights { 2.0, 4.0, 6.0, 8.0 };

        SUBCASE("batch")
        {
            auto stats = univariate::accumulate<float>(values.begin(), values.end());
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("batch weighted")
        {
            auto stats = univariate::accumulate<float>(values.begin(), values.end(), weights.begin());
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("batch projection")
        {
            struct Foo {
                float value;
            };

            Foo foos[] = { { 1 }, { 3 }, { 5 }, { 2 }, { 8 } };
            auto stats = univariate::accumulate<float>(foos, std::size(foos), [](auto const& foo) { return foo.value; });
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("batch binary op projection")
        {
            auto stats = univariate::accumulate<float>(values.begin(), values.end(), weights.begin(), [](auto v, auto w) { return (v - w) * (v - w); });
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("accumulator")
        {
            univariate_accumulator<float> acc(1.0);
            acc(2.0);
            acc(3.0);
            acc(4.0);
            auto stats = univariate_statistics(acc);
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("accumulator weighted")
        {
            univariate_accumulator<float> acc(1.0, 2.0);
            acc(2.0, 4.0);
            acc(3.0, 6.0);
            auto stats = univariate_statistics(acc);
            std::cout << "stats:\n"
                      << stats << "\n";
        }
    }

    TEST_CASE("bivariate")
    {
        float x[] = { 1., 1., 2., 6. };
        float y[] = { 2., 4., 3., 1. };
        size_t n = std::size(x);

        SUBCASE("batch")
        {
            auto stats = bivariate::accumulate<float>(x, y, n);
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("batch projection")
        {
            struct Foo {
                float value;
            };

            struct Bar {
                int value;
            };

            Foo foos[] = { { 1 }, { 3 }, { 5 }, { 2 }, { 8 } };
            Bar bars[] = { { 3 }, { 2 }, { 1 }, { 4 }, { 11 } };

            auto stats = bivariate::accumulate<float>(
                foos, bars, std::size(foos), [](auto const& foo) { return foo.value; }, [](auto const& bar) { return bar.value; });
            std::cout << "stats:\n"
                      << stats << "\n";
        }

        SUBCASE("accumulator")
        {
            bivariate_accumulator<float> acc(x[0], y[0]);
            for (size_t i = 1; i < n; ++i) {
                acc(x[i], y[i]);
            }
            bivariate_statistics stats(acc);
            std::cout << "stats:\n"
                      << stats << "\n";
        }
    }
}

TEST_SUITE("correctness")
{
    TEST_CASE("univariate")
    {
        const int n = int(1e6);

        std::vector<float> xf(n);
        std::vector<double> xd(n);

        std::vector<float> yf(n);
        std::vector<double> yd(n);

        std::vector<float> wf(n);
        std::vector<double> wd(n);

        std::default_random_engine rng(1234);
        std::uniform_real_distribution<double> dist(-1, 1);

        std::generate(xd.begin(), xd.end(), [&]() { return dist(rng); });
        std::generate(yd.begin(), yd.end(), [&]() { return dist(rng); });

        std::copy(xd.begin(), xd.end(), xf.begin());
        std::copy(yd.begin(), yd.end(), yf.begin());

        std::vector<Foo> ff(n);
        for (int i = 0; i < n; ++i) {
            ff[i].value = xd[i];
        }

        auto gsl_var_flt = gsl_stats_float_variance(xf.data(), 1, xf.size());
        auto gsl_mean_flt = gsl_stats_float_mean(xf.data(), 1, xf.size());

        SUBCASE("float")
        {
            auto stats = univariate::accumulate<float>(xd.begin(), xd.end(), detail::identity {});
            CHECK(std::abs(stats.mean - gsl_mean_flt) < 1e-6);
            CHECK(std::abs(stats.variance - gsl_var_flt) < 1e-5);
        }

        SUBCASE("float weighted")
        {
            // now test the weighted version with weights set to 1
            std::fill(wf.begin(), wf.end(), 1.0);
            auto stats = univariate::accumulate<float>(xf.begin(), xf.end(), wf.begin());
            CHECK(std::abs(stats.mean - gsl_mean_flt) < 1e-6);
            CHECK(std::abs(stats.variance - gsl_var_flt) < 1e-5);

            std::fill(wf.begin(), wf.end(), 0.2);
            auto gsl_wmean_flt = gsl_stats_float_wmean(wf.data(), 1, xf.data(), 1, n);
            auto gsl_wvar_flt = gsl_stats_float_wvariance(wf.data(), 1, xf.data(), 1, n);
            stats = univariate::accumulate<float>(xf.begin(), xf.end(), wf.begin());
            CHECK(std::abs(stats.mean - gsl_wmean_flt) < 1e-5);
            CHECK(std::abs(stats.variance - gsl_wvar_flt) < 1e-5);

            ba::accumulator_set<float, ba::stats<ba::tag::weighted_variance>, float> acc;
            for (size_t i = 0; i < n; ++i) {
                acc(xf[i], ba::weight = wf[i]);
            }
            auto ba_wvar_flt = ba::weighted_variance(acc);
            CHECK(std::abs(ba_wvar_flt - gsl_wvar_flt) < 1e-6);

            float x[] { 2, 2, 4, 5, 5, 5 };
            float y[] { 2, 4, 5 };
            float w[] { 2, 1, 3 };

            auto stats1 = univariate::accumulate<float>(x, std::size(x));
            auto stats2 = univariate::accumulate<float>(y, w, std::size(y));
            CHECK(stats1.mean == stats2.mean);
            CHECK(std::abs(stats1.variance - stats2.variance) < 1e-5);

            univariate_accumulator<float> a1(x[0]);
            univariate_accumulator<float> a2(y[0], w[0]);
            for (size_t i = 1; i < std::size(x); ++i)
                a1(x[i]);
            for (size_t i = 1; i < std::size(y); ++i)
                a2(y[i], w[i]);
            CHECK(std::abs(univariate_statistics(a1).variance - univariate_statistics(a2).variance) < 1e-6);

            auto stats3 = univariate::accumulate<float>(y, w, std::size(y), std::multiplies<float> {});
            CHECK(stats3.sum == stats2.sum);
        }

        SUBCASE("double")
        {
            auto gsl_var_dbl = gsl_stats_variance(xd.data(), 1, xd.size());
            auto gsl_mean_dbl = gsl_stats_mean(xd.data(), 1, xd.size());
            auto stats = univariate::accumulate<double>(xd.begin(), xd.end(), detail::identity {});
            CHECK(std::abs(stats.mean - gsl_mean_dbl) < 1e-6);
            CHECK(std::abs(stats.variance - gsl_var_dbl) < 1e-6);
        }
    }

    TEST_CASE("bivariate")
    {
        const int n = int(1e6);

        std::vector<float> xf(n);
        std::vector<double> xd(n);

        std::vector<float> yf(n);
        std::vector<double> yd(n);

        std::vector<float> wf(n);
        std::vector<double> wd(n);

        std::default_random_engine rng(1234);
        std::uniform_real_distribution<double> dist(-1, 1);

        std::generate(xd.begin(), xd.end(), [&]() { return dist(rng); });
        std::generate(yd.begin(), yd.end(), [&]() { return dist(rng); });

        std::copy(xd.begin(), xd.end(), xf.begin());
        std::copy(yd.begin(), yd.end(), yf.begin());

        std::vector<Foo> ff(n);
        for (int i = 0; i < n; ++i) {
            ff[i].value = xd[i];
        }

        auto gsl_corr_flt = gsl_stats_float_correlation(xf.data(), 1, yf.data(), 1, n);
        auto gsl_corr_dbl = gsl_stats_correlation(xd.data(), 1, yd.data(), 1, n);

        auto gsl_cov_flt = gsl_stats_float_covariance(xf.data(), 1, yf.data(), 1, n);
        auto gsl_cov_dbl = gsl_stats_covariance(xd.data(), 1, yd.data(), 1, n);

        auto bstats = bivariate::accumulate<float>(xd.begin(), xd.end(), yd.begin());
        CHECK(std::abs(gsl_corr_flt - bstats.correlation) < 1e-6);
        CHECK(std::abs(gsl_cov_flt - bstats.covariance) < 1e-6);

        bstats = bivariate::accumulate<float>(xd.begin(), xd.end(), yd.begin());
        CHECK(std::abs(gsl_corr_dbl - bstats.correlation) < 1e-6);
        CHECK(std::abs(gsl_cov_dbl - bstats.covariance) < 1e-6);

        auto stats_x = univariate::accumulate<float>(xd.begin(), xd.end());
        auto stats_y = univariate::accumulate<float>(yd.begin(), yd.end());

        CHECK(bstats.mean_x == stats_x.mean);
        CHECK(bstats.mean_y == stats_y.mean);
        CHECK(bstats.sum_x == stats_x.sum);
        CHECK(bstats.sum_y == stats_y.sum);
    }
}

TEST_SUITE("performance")
{
    TEST_CASE("univariate")
    {
        const int n = int(1e6);

        std::vector<double> v1(n);
        std::vector<double> v2(n);
        std::vector<double> v3(n);
        std::vector<float> u1(n);
        std::vector<float> u2(n);
        std::vector<float> u3(n);

        auto *xd = v1.data();
        auto *yd = v2.data();
        auto *wd = v3.data();
        auto *xf = u1.data();
        auto *yf = u2.data();
        auto *wf = u3.data();

        std::default_random_engine rng(1234);
        std::uniform_real_distribution<double> dist(-1, 1);

        std::generate(xd, xd + n, [&]() { return dist(rng); });
        std::generate(yd, yd + n, [&]() { return dist(rng); });
        std::generate(wd, wd + n, [&]() { return dist(rng); });

        std::copy(xd, xd + n, xf);
        std::copy(yd, yd + n, yf);
        std::copy(wd, wd + n, wf);

        std::vector<Foo> ff(n);
        for (int i = 0; i < n; ++i) {
            ff[i].value = xd[i];
        }

        ankerl::nanobench::Bench b;
        b.performanceCounters(true).minEpochIterations(100).batch(n);

        // print some runtime stats for different data sizes
        std::vector<int> sizes { 1000, 10000 };
        int step = int(1e5);
        for (int s = step; s <= n; s += step) {
            sizes.push_back(s);
        }

        SUBCASE("vstat accumulator")
        {
            double var, count;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat acc variance float " + std::to_string(s), [&]() {
                    ++count;
                    univariate_accumulator<Vec8f> acc(Vec8f().load(xf));
                    constexpr auto sz = Vec8f::size();
                    size_t m = s & (-sz);
                    for (size_t i = sz; i < m; i += sz) {
                        acc(Vec8f().load(xf + i));
                    }
                    var += univariate_statistics(acc).variance;
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat acc variance float weighted " + std::to_string(s), [&]() {
                    ++count;
                    univariate_accumulator<Vec8f> acc(Vec8f().load(xf), Vec8f().load(wf));
                    constexpr auto sz = Vec8f::size();
                    size_t m = s & (-sz);
                    for (size_t i = sz; i < m; i += sz) {
                        acc(Vec8f().load(xf + i), Vec8f().load(wf + i));
                    }
                    var += univariate_statistics(acc).variance;
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat acc variance double " + std::to_string(s), [&]() {
                    ++count;
                    univariate_accumulator<Vec4d> acc(Vec4d().load(xd));
                    constexpr auto sz = Vec4d::size();
                    size_t m = s & (-sz);
                    for (size_t i = sz; i < m; i += sz) {
                        acc(Vec4d().load(xd + i));
                    }
                    var += univariate_statistics(acc).variance;
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat acc variance double weighted " + std::to_string(s), [&]() {
                    ++count;
                    univariate_accumulator<Vec4d> acc(Vec4d().load(xd), Vec4d().load(wd));
                    constexpr auto sz = Vec4d::size();
                    size_t m = s & (-sz);
                    for (size_t i = sz; i < m; i += sz) {
                        acc(Vec4d().load(xd + i), Vec4d().load(wd + i));
                    }
                    var += univariate_statistics(acc).variance;
                });
            }
        }

        SUBCASE("vstat")
        {
            double var = 0, count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat variance float " + std::to_string(s), [&]() {
                    ++count;
                    var += univariate::accumulate<float>(xf, s).variance;
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat variance double " + std::to_string(s), [&]() {
                    ++count;
                    var += univariate::accumulate<double>(xd, s).variance;
                });
            }
        }

        SUBCASE("vstat weighted")
        {
            double var = 0, count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat variance float weighted " + std::to_string(s), [&]() {
                    ++count;
                    var += univariate::accumulate<float>(xf, wf, s).variance;
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat variance double weighted " + std::to_string(s), [&]() {
                    ++count;
                    var += univariate::accumulate<double>(xd, wd, s).variance;
                });
            }
        }

        SUBCASE("linasm")
        {
            double var = 0, count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("linasm variance float " + std::to_string(s), [&]() {
                    ++count;
                    auto mean = Statistics::Mean(xf, s);
                    var += Statistics::Variance(xf, s, mean);
                });
            }

            for (auto s : sizes) {
                b.batch(s).run("linasm variance double " + std::to_string(s), [&]() {
                    ++count;
                    auto mean = Statistics::Mean(xd, s);
                    var += Statistics::Variance(xd, s, mean);
                });
            }
        }

        SUBCASE("boost accumulators")
        {
            double var = 0, count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("boost variance float " + std::to_string(s), [&]() {
                    ++count;
                    ba::accumulator_set<float, ba::features<ba::tag::variance>> acc;
                    for (int i = 0; i < s; ++i) {
                        acc(xf[i]);
                    }
                    var += ba::variance(acc);
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("boost variance double " + std::to_string(s), [&]() {
                    ++count;
                    ba::accumulator_set<double, ba::features<ba::tag::variance>> acc;
                    for (int i = 0; i < s; ++i) {
                        acc(xd[i]);
                    }
                    var += ba::variance(acc);
                });
            }
        }

        SUBCASE("gsl")
        {
            double var = 0, count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("gsl variance float " + std::to_string(s), [&]() {
                    ++count;
                    var += gsl_stats_float_variance(xf, 1, s);
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("gsl variance double " + std::to_string(s), [&]() {
                    ++count;
                    var += gsl_stats_variance(xd, 1, s);
                });
            }
        }
    }

    TEST_CASE("bivariate")
    {
        const int n = int(1e6);

        std::vector<double> v1(n), v2(n);
        std::vector<float> u1(n), u2(n);

        auto xd = v1.data();
        auto yd = v2.data();
        auto xf = u1.data();
        auto yf = u2.data();

        std::default_random_engine rng(1234);
        std::uniform_real_distribution<double> dist(-1, 1);

        std::generate(xd, xd + n, [&]() { return dist(rng); });
        std::generate(yd, yd + n, [&]() { return dist(rng); });

        std::copy(xd, xd + n, xf);
        std::copy(yd, yd + n, yf);

        std::vector<Foo> ff(n);
        for (int i = 0; i < n; ++i) {
            ff[i].value = xd[i];
        }

        ankerl::nanobench::Bench b;
        b.performanceCounters(true).minEpochIterations(100).batch(n);

        // print some runtime stats for different data sizes
        std::vector<int> sizes { 1000, 10000 };
        int step = int(1e5);
        for (int s = step; s <= n; s += step) {
            sizes.push_back(s);
        }

        SUBCASE("vstat")
        {
            double var = 0, count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat covariance float " + std::to_string(s), [&]() {
                    ++count;
                    var += bivariate::accumulate<float>(xf, yf, s).covariance;
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("vstat covariance double " + std::to_string(s), [&]() {
                    ++count;
                    var += bivariate::accumulate<double>(xd, yd, s).covariance;
                });
            }
        }

        SUBCASE("vstat")
        {
            double var = 0, count = 0;
            var = count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("linasm covariance float " + std::to_string(s), [&]() {
                    ++count;
                    auto xm = Statistics::Mean(xf, s);
                    auto ym = Statistics::Mean(yf, s);
                    var += Statistics::Covariance(xf, yf, s, xm, ym);
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("linasm covariance double " + std::to_string(s), [&]() {
                    ++count;
                    auto xm = Statistics::Mean(xd, s);
                    auto ym = Statistics::Mean(yd, s);
                    var += Statistics::Covariance(xd, yd, s, xm, ym);
                });
            }
        }

        SUBCASE("vstat")
        {
            double var = 0, count = 0;
            var = count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("boost covariance float " + std::to_string(s), [&]() {
                    ++count;
                    ba::accumulator_set<float, ba::stats<ba::tag::covariance<float, ba::tag::covariate1>>> acc;
                    for (int i = 0; i < s; ++i) {
                        acc(xf[i], ba::covariate1 = yf[i]);
                    }
                    var += ba::covariance(acc);
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("boost covariance double " + std::to_string(s), [&]() {
                    ++count;
                    ba::accumulator_set<double, ba::stats<ba::tag::covariance<double, ba::tag::covariate1>>> acc;
                    for (int i = 0; i < s; ++i) {
                        acc(xd[i], ba::covariate1 = yd[i]);
                    }
                    var += ba::covariance(acc);
                });
            }
        }

        SUBCASE("vstat")
        {
            double var = 0, count = 0;
            var = count = 0;
            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("gsl covariance float " + std::to_string(s), [&]() {
                    ++count;
                    var += gsl_stats_float_covariance(xf, 1, yf, 1, s);
                });
            }

            for (auto s : sizes) {
                var = count = 0;
                b.batch(s).run("gsl covariance double " + std::to_string(s), [&]() {
                    ++count;
                    var += gsl_stats_covariance(xd, 1, yd, 1, s);
                });
            }
        }
    }
}


// ----------------------------------------------------------------------------
// -                        Open3D: www.open3d.org                            -
// ----------------------------------------------------------------------------
// The MIT License (MIT)
//
// Copyright (c) 2019 www.open3d.org
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// ----------------------------------------------------------------------------

#include <Eigen/Dense>
#include <queue>
#include <tuple>

#include "open3d/geometry/TriangleMesh.h"
#include "open3d/utility/Console.h"

namespace open3d {
namespace geometry {

/// Error quadric that is used to minimize the squared distance of a point to
/// its neigbhouring triangle planes.
/// Cf. "Simplifying Surfaces with Color and Texture using Quadric Error
/// Metrics" by Garland and Heckbert.
class Quadric {
public:
    Quadric() {
        A_.fill(0);
        b_.fill(0);
        c_ = 0;
    }

    Quadric(const Eigen::Vector4d& plane, double weight = 1) {
        Eigen::Vector3d n = plane.head<3>();
        A_ = weight * n * n.transpose();
        b_ = weight * plane(3) * n;
        c_ = weight * plane(3) * plane(3);
    }

    Quadric& operator+=(const Quadric& other) {
        A_ += other.A_;
        b_ += other.b_;
        c_ += other.c_;
        return *this;
    }

    Quadric operator+(const Quadric& other) const {
        Quadric res;
        res.A_ = A_ + other.A_;
        res.b_ = b_ + other.b_;
        res.c_ = c_ + other.c_;
        return res;
    }

    double Eval(const Eigen::Vector3d& v) const {
        Eigen::Vector3d Av = A_ * v;
        double q = v.dot(Av) + 2 * b_.dot(v) + c_;
        return q;
    }

    bool IsInvertible() const { return std::fabs(A_.determinant()) > 1e-4; }

    Eigen::Vector3d Minimum() const { return -A_.ldlt().solve(b_); }

public:
    /// A_ = n . n^T, where n is the plane normal
    Eigen::Matrix3d A_;
    /// b_ = d . n, where n is the plane normal and d the non-normal component
    /// of the plane parameters
    Eigen::Vector3d b_;
    /// c_ = d . d, where d the non-normal component pf the plane parameters
    double c_;
};

std::shared_ptr<TriangleMesh> TriangleMesh::SimplifyVertexClustering(
        double voxel_size,
        SimplificationContraction
                contraction /* = SimplificationContraction::Average */) const {
    if (HasTriangleUvs()) {
        utility::LogWarning(
                "[SimplifyVertexClustering] This mesh contains triangle uvs "
                "that are not handled in this function");
    }
    auto mesh = std::make_shared<TriangleMesh>();
    if (voxel_size <= 0.0) {
        utility::LogError("[VoxelGridFromPointCloud] voxel_size <= 0.0");
    }

    Eigen::Vector3d voxel_size3 =
            Eigen::Vector3d(voxel_size, voxel_size, voxel_size);
    Eigen::Vector3d voxel_min_bound = GetMinBound() - voxel_size3 * 0.5;
    Eigen::Vector3d voxel_max_bound = GetMaxBound() + voxel_size3 * 0.5;
    if (voxel_size * std::numeric_limits<int>::max() <
        (voxel_max_bound - voxel_min_bound).maxCoeff()) {
        utility::LogError("[VoxelGridFromPointCloud] voxel_size is too small.");
    }

    auto GetVoxelIdx = [&](const Eigen::Vector3d& vert) {
        Eigen::Vector3d ref_coord = (vert - voxel_min_bound) / voxel_size;
        Eigen::Vector3i idx(int(floor(ref_coord(0))), int(floor(ref_coord(1))),
                            int(floor(ref_coord(2))));
        return idx;
    };

    std::unordered_map<Eigen::Vector3i, std::unordered_set<int>,
                       utility::hash_eigen<Eigen::Vector3i>>
            voxel_vertices;
    std::unordered_map<Eigen::Vector3i, int,
                       utility::hash_eigen<Eigen::Vector3i>>
            voxel_vert_ind;
    int new_vidx = 0;
    for (size_t vidx = 0; vidx < vertices_.size(); ++vidx) {
        const Eigen::Vector3i vox_idx = GetVoxelIdx(vertices_[vidx]);
        voxel_vertices[vox_idx].insert(int(vidx));

        if (voxel_vert_ind.count(vox_idx) == 0) {
            voxel_vert_ind[vox_idx] = new_vidx;
            new_vidx++;
        }
    }

    // aggregate vertex info
    bool has_vert_normal = HasVertexNormals();
    bool has_vert_color = HasVertexColors();
    mesh->vertices_.resize(voxel_vertices.size());
    if (has_vert_normal) {
        mesh->vertex_normals_.resize(voxel_vertices.size());
    }
    if (has_vert_color) {
        mesh->vertex_colors_.resize(voxel_vertices.size());
    }

    auto AvgVertex = [&](const std::unordered_set<int> ind) {
        Eigen::Vector3d aggr(0, 0, 0);
        for (int vidx : ind) {
            aggr += vertices_[vidx];
        }
        aggr /= double(ind.size());
        return aggr;
    };
    auto AvgNormal = [&](const std::unordered_set<int> ind) {
        Eigen::Vector3d aggr(0, 0, 0);
        for (int vidx : ind) {
            aggr += vertex_normals_[vidx];
        }
        aggr /= double(ind.size());
        return aggr;
    };
    auto AvgColor = [&](const std::unordered_set<int> ind) {
        Eigen::Vector3d aggr(0, 0, 0);
        for (int vidx : ind) {
            aggr += vertex_colors_[vidx];
        }
        aggr /= double(ind.size());
        return aggr;
    };

    if (contraction == SimplificationContraction::Average) {
        for (const auto& voxel : voxel_vertices) {
            int vox_vidx = voxel_vert_ind[voxel.first];
            mesh->vertices_[vox_vidx] = AvgVertex(voxel.second);
            if (has_vert_normal) {
                mesh->vertex_normals_[vox_vidx] = AvgNormal(voxel.second);
            }
            if (has_vert_color) {
                mesh->vertex_colors_[vox_vidx] = AvgColor(voxel.second);
            }
        }
    } else if (contraction == SimplificationContraction::Quadric) {
        // Map triangles
        std::unordered_map<int, std::unordered_set<int>> vert_to_triangles;
        for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
            vert_to_triangles[triangles_[tidx](0)].emplace(int(tidx));
            vert_to_triangles[triangles_[tidx](1)].emplace(int(tidx));
            vert_to_triangles[triangles_[tidx](2)].emplace(int(tidx));
        }

        for (const auto& voxel : voxel_vertices) {
            int vox_vidx = voxel_vert_ind[voxel.first];

            Quadric q;
            for (int vidx : voxel.second) {
                for (int tidx : vert_to_triangles[vidx]) {
                    Eigen::Vector4d p = GetTrianglePlane(tidx);
                    double area = GetTriangleArea(tidx);
                    q += Quadric(p, area);
                }
            }
            if (q.IsInvertible()) {
                Eigen::Vector3d v = q.Minimum();
                mesh->vertices_[vox_vidx] = v;
            } else {
                mesh->vertices_[vox_vidx] = AvgVertex(voxel.second);
            }

            if (has_vert_normal) {
                mesh->vertex_normals_[vox_vidx] = AvgNormal(voxel.second);
            }
            if (has_vert_color) {
                mesh->vertex_colors_[vox_vidx] = AvgColor(voxel.second);
            }
        }
    }

    //  connect vertices
    std::unordered_set<Eigen::Vector3i, utility::hash_eigen<Eigen::Vector3i>>
            triangles;
    for (const auto& triangle : triangles_) {
        int vidx0 = voxel_vert_ind[GetVoxelIdx(vertices_[triangle(0)])];
        int vidx1 = voxel_vert_ind[GetVoxelIdx(vertices_[triangle(1)])];
        int vidx2 = voxel_vert_ind[GetVoxelIdx(vertices_[triangle(2)])];

        // only connect if in different voxels
        if (vidx0 == vidx1 || vidx0 == vidx2 || vidx1 == vidx2) {
            continue;
        }

        // Note: there can be still double faces with different orientation
        // The user has to clean up manually
        if (vidx1 < vidx0 && vidx1 < vidx2) {
            int tmp = vidx0;
            vidx0 = vidx1;
            vidx1 = vidx2;
            vidx2 = tmp;
        } else if (vidx2 < vidx0 && vidx2 < vidx1) {
            int tmp = vidx1;
            vidx1 = vidx0;
            vidx0 = vidx2;
            vidx2 = tmp;
        }

        triangles.emplace(Eigen::Vector3i(vidx0, vidx1, vidx2));
    }

    mesh->triangles_.resize(triangles.size());
    int tidx = 0;
    for (const Eigen::Vector3i& triangle : triangles) {
        mesh->triangles_[tidx] = triangle;
        tidx++;
    }

    if (HasTriangleNormals()) {
        mesh->ComputeTriangleNormals();
    }

    return mesh;
}

std::shared_ptr<TriangleMesh> TriangleMesh::SimplifyQuadricDecimation(
        int target_number_of_triangles) const {
    if (HasTriangleUvs()) {
        utility::LogWarning(
                "[SimplifyQuadricDecimation] This mesh contains triangle uvs "
                "that are not handled in this function");
    }
    typedef std::tuple<double, int, int> CostEdge;

    auto mesh = std::make_shared<TriangleMesh>();
    mesh->vertices_ = vertices_;
    mesh->vertex_normals_ = vertex_normals_;
    mesh->vertex_colors_ = vertex_colors_;
    mesh->triangles_ = triangles_;

    std::vector<bool> vertices_deleted(vertices_.size(), false);
    std::vector<bool> triangles_deleted(triangles_.size(), false);

    // Map vertices to triangles and compute triangle planes and areas
    std::vector<std::unordered_set<int>> vert_to_triangles(vertices_.size());
    std::vector<Eigen::Vector4d> triangle_planes(triangles_.size());
    std::vector<double> triangle_areas(triangles_.size());
    for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
        vert_to_triangles[triangles_[tidx](0)].emplace(static_cast<int>(tidx));
        vert_to_triangles[triangles_[tidx](1)].emplace(static_cast<int>(tidx));
        vert_to_triangles[triangles_[tidx](2)].emplace(static_cast<int>(tidx));

        triangle_planes[tidx] = GetTrianglePlane(tidx);
        triangle_areas[tidx] = GetTriangleArea(tidx);
    }

    // Compute the error metric per vertex
    std::vector<Quadric> Qs(vertices_.size());
    for (size_t vidx = 0; vidx < vertices_.size(); ++vidx) {
        for (int tidx : vert_to_triangles[vidx]) {
            Qs[vidx] += Quadric(triangle_planes[tidx], triangle_areas[tidx]);
        }
    }

    // For boundary edges add perpendicular plane quadric
    auto edge_triangle_count = GetEdgeToTrianglesMap();
    auto AddPerpPlaneQuadric = [&](int vidx0, int vidx1, int vidx2,
                                   double area) {
        int min = std::min(vidx0, vidx1);
        int max = std::max(vidx0, vidx1);
        Eigen::Vector2i edge(min, max);
        if (edge_triangle_count[edge].size() != 1) {
            return;
        }
        const auto& vert0 = mesh->vertices_[vidx0];
        const auto& vert1 = mesh->vertices_[vidx1];
        const auto& vert2 = mesh->vertices_[vidx2];
        Eigen::Vector3d vert2p = (vert2 - vert0).cross(vert2 - vert1);
        Eigen::Vector4d plane = ComputeTrianglePlane(vert0, vert1, vert2p);
        Quadric quad(plane, area);
        Qs[vidx0] += quad;
        Qs[vidx1] += quad;
    };
    for (size_t tidx = 0; tidx < triangles_.size(); ++tidx) {
        const auto& tria = triangles_[tidx];
        double area = triangle_areas[tidx];
        AddPerpPlaneQuadric(tria(0), tria(1), tria(2), area);
        AddPerpPlaneQuadric(tria(1), tria(2), tria(0), area);
        AddPerpPlaneQuadric(tria(2), tria(0), tria(1), area);
    }

    // Get valid edges and compute cost
    // Note: We could also select all vertex pairs as edges with dist < eps
    std::unordered_map<Eigen::Vector2i, Eigen::Vector3d,
                       utility::hash_eigen<Eigen::Vector2i>>
            vbars;
    std::unordered_map<Eigen::Vector2i, double,
                       utility::hash_eigen<Eigen::Vector2i>>
            costs;
    auto CostEdgeComp = [](const CostEdge& a, const CostEdge& b) {
        return std::get<0>(a) > std::get<0>(b);
    };
    std::priority_queue<CostEdge, std::vector<CostEdge>, decltype(CostEdgeComp)>
            queue(CostEdgeComp);

    auto AddEdge = [&](int vidx0, int vidx1, bool update) {
        int min = std::min(vidx0, vidx1);
        int max = std::max(vidx0, vidx1);
        Eigen::Vector2i edge(min, max);
        if (update || vbars.count(edge) == 0) {
            const Quadric& Q0 = Qs[min];
            const Quadric& Q1 = Qs[max];
            Quadric Qbar = Q0 + Q1;
            double cost;
            Eigen::Vector3d vbar;
            if (Qbar.IsInvertible()) {
                vbar = Qbar.Minimum();
                cost = Qbar.Eval(vbar);
            } else {
                const Eigen::Vector3d& v0 = mesh->vertices_[vidx0];
                const Eigen::Vector3d& v1 = mesh->vertices_[vidx0];
                Eigen::Vector3d vmid = (v0 + v1) / 2;
                double cost0 = Qbar.Eval(v0);
                double cost1 = Qbar.Eval(v1);
                double costmid = Qbar.Eval(vbar);
                cost = std::min(cost0, std::min(cost1, costmid));
                if (cost == costmid) {
                    vbar = vmid;
                } else if (cost == cost0) {
                    vbar = v0;
                } else {
                    vbar = v1;
                }
            }
            vbars[edge] = vbar;
            costs[edge] = cost;
            queue.push(CostEdge(cost, min, max));
        }
    };

    // add all edges to priority queue
    for (const auto& triangle : triangles_) {
        AddEdge(triangle(0), triangle(1), false);
        AddEdge(triangle(1), triangle(2), false);
        AddEdge(triangle(2), triangle(0), false);
    }

    // perform incremental edge collapse
    bool has_vert_normal = HasVertexNormals();
    bool has_vert_color = HasVertexColors();
    int n_triangles = int(triangles_.size());
    while (n_triangles > target_number_of_triangles && !queue.empty()) {
        // retrieve edge from queue
        double cost;
        int vidx0, vidx1;
        std::tie(cost, vidx0, vidx1) = queue.top();
        queue.pop();

        // test if the edge has been updated (reinserted into queue)
        Eigen::Vector2i edge(vidx0, vidx1);
        bool valid = !vertices_deleted[vidx0] && !vertices_deleted[vidx1] &&
                     cost == costs[edge];
        if (!valid) {
            continue;
        }

        // avoid flip of triangle normal
        bool flipped = false;
        for (int tidx : vert_to_triangles[vidx1]) {
            if (triangles_deleted[tidx]) {
                continue;
            }

            const Eigen::Vector3i& tria = mesh->triangles_[tidx];
            bool has_vidx0 =
                    vidx0 == tria(0) || vidx0 == tria(1) || vidx0 == tria(2);
            bool has_vidx1 =
                    vidx1 == tria(0) || vidx1 == tria(1) || vidx1 == tria(2);
            if (has_vidx0 && has_vidx1) {
                continue;
            }

            Eigen::Vector3d vert0 = mesh->vertices_[tria(0)];
            Eigen::Vector3d vert1 = mesh->vertices_[tria(1)];
            Eigen::Vector3d vert2 = mesh->vertices_[tria(2)];
            Eigen::Vector3d norm_before = (vert1 - vert0).cross(vert2 - vert0);
            norm_before /= norm_before.norm();

            if (vidx1 == tria(0)) {
                vert0 = vbars[edge];
            } else if (vidx1 == tria(1)) {
                vert1 = vbars[edge];
            } else if (vidx1 == tria(2)) {
                vert2 = vbars[edge];
            }

            Eigen::Vector3d norm_after = (vert1 - vert0).cross(vert2 - vert0);
            norm_after /= norm_after.norm();
            if (norm_before.dot(norm_after) < 0) {
                flipped = true;
                break;
            }
        }
        if (flipped) {
            continue;
        }

        // Connect triangles from vidx1 to vidx0, or mark deleted
        for (int tidx : vert_to_triangles[vidx1]) {
            if (triangles_deleted[tidx]) {
                continue;
            }

            Eigen::Vector3i& tria = mesh->triangles_[tidx];
            bool has_vidx0 =
                    vidx0 == tria(0) || vidx0 == tria(1) || vidx0 == tria(2);
            bool has_vidx1 =
                    vidx1 == tria(0) || vidx1 == tria(1) || vidx1 == tria(2);

            if (has_vidx0 && has_vidx1) {
                triangles_deleted[tidx] = true;
                n_triangles--;
                continue;
            }

            if (vidx1 == tria(0)) {
                tria(0) = vidx0;
            } else if (vidx1 == tria(1)) {
                tria(1) = vidx0;
            } else if (vidx1 == tria(2)) {
                tria(2) = vidx0;
            }
            vert_to_triangles[vidx0].insert(tidx);
        }

        // update vertex vidx0 to vbar
        mesh->vertices_[vidx0] = vbars[edge];
        Qs[vidx0] += Qs[vidx1];
        if (has_vert_normal) {
            mesh->vertex_normals_[vidx0] = 0.5 * (mesh->vertex_normals_[vidx0] +
                                                  mesh->vertex_normals_[vidx1]);
        }
        if (has_vert_color) {
            mesh->vertex_colors_[vidx0] = 0.5 * (mesh->vertex_colors_[vidx0] +
                                                 mesh->vertex_colors_[vidx1]);
        }
        vertices_deleted[vidx1] = true;

        // Update edge costs for all triangles connecting to vidx0
        for (const auto& tidx : vert_to_triangles[vidx0]) {
            if (triangles_deleted[tidx]) {
                continue;
            }
            const Eigen::Vector3i& tria = mesh->triangles_[tidx];
            if (tria(0) == vidx0 || tria(1) == vidx0) {
                AddEdge(tria(0), tria(1), true);
            }
            if (tria(1) == vidx0 || tria(2) == vidx0) {
                AddEdge(tria(1), tria(2), true);
            }
            if (tria(2) == vidx0 || tria(0) == vidx0) {
                AddEdge(tria(2), tria(0), true);
            }
        }
    }

    // Apply changes to the triangle mesh
    int next_free = 0;
    std::unordered_map<int, int> vert_remapping;
    for (size_t idx = 0; idx < mesh->vertices_.size(); ++idx) {
        if (!vertices_deleted[idx]) {
            vert_remapping[int(idx)] = next_free;
            mesh->vertices_[next_free] = mesh->vertices_[idx];
            if (has_vert_normal) {
                mesh->vertex_normals_[next_free] = mesh->vertex_normals_[idx];
            }
            if (has_vert_color) {
                mesh->vertex_colors_[next_free] = mesh->vertex_colors_[idx];
            }
            next_free++;
        }
    }
    mesh->vertices_.resize(next_free);
    if (has_vert_normal) {
        mesh->vertex_normals_.resize(next_free);
    }
    if (has_vert_color) {
        mesh->vertex_colors_.resize(next_free);
    }

    next_free = 0;
    for (size_t idx = 0; idx < mesh->triangles_.size(); ++idx) {
        if (!triangles_deleted[idx]) {
            Eigen::Vector3i tria = mesh->triangles_[idx];
            mesh->triangles_[next_free](0) = vert_remapping[tria(0)];
            mesh->triangles_[next_free](1) = vert_remapping[tria(1)];
            mesh->triangles_[next_free](2) = vert_remapping[tria(2)];
            next_free++;
        }
    }
    mesh->triangles_.resize(next_free);

    if (HasTriangleNormals()) {
        mesh->ComputeTriangleNormals();
    }

    return mesh;
}

}  // namespace geometry
}  // namespace open3d


#define BOOST_TEST_MODULE SolutionTest

#include "solution.hpp"

#define BOOST_TEST_DYN_LINK
#include <boost/test/unit_test.hpp>

BOOST_AUTO_TEST_SUITE(SolutionSuite)

BOOST_AUTO_TEST_CASE(PlainTest1)
{
    vector<int> height{1,8,6,2,5,4,8,3,7};
    int results = Solution().maxArea(height);

    int expected = 49;
    BOOST_CHECK_EQUAL(results, expected);
}

BOOST_AUTO_TEST_SUITE_END()


//==================================================================================================
/*!
  @file

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-ieee
    Function object implementing modf capabilities

    Computes the integer and  fractional parts of the input

    @par Semantic:

    @code
    T t = modf(x, f);
    @endcode

    is similar to:

    @code
    T t = trunc(x);
    T f = frac(x);
    @endcode

    The following call can also be used

    @code
    std::pair<T,T> p = modf(x);
    @endcode

    @see frac,  trunc

  **/
  const boost::dispatch::functor<tag::modf_> modf = {};
} }
#endif

#include <boost/simd/function/scalar/modf.hpp>
#include <boost/simd/function/simd/modf.hpp>

#endif


// Software License for MTL
//
// Copyright (c) 2007 The Trustees of Indiana University.
//               2008 Dresden University of Technology and the Trustees of Indiana University.
//               2010 SimuNova UG (haftungsbeschrÃ¤nkt), www.simunova.com.
// All rights reserved.
// Authors: Peter Gottschling and Andrew Lumsdaine
//
// This file is part of the Matrix Template Library
//
// See also license.mtl.txt in the distribution.

// #define MTL_VERBOSE_TEST

#include <iostream>
#include <boost/tuple/tuple.hpp>

#include <boost/numeric/mtl/mtl.hpp>
#include <boost/numeric/mtl/utility/tag.hpp>
#include <boost/numeric/mtl/matrix/coordinate2D.hpp>

int main(int, char**)
{
    using namespace mtl;
    using mtl::io::tout;

    typedef mtl::mat::coordinate2D<double> matrix_type;
    matrix_type   B(5, 4);
    mtl::dense_vector<double> res(5, 0.0), res2(5), x(4, 1.0);

    tout << "num_rows = " << B.num_rows() << "\n"
	 << "num_rows = " << num_rows(B) << "\n"
	 << "num_cols = " << num_cols(B) << "\n"
	 << "size = " << size(B) << "\n"
	 << "nnz = " << nnz(B) << "\n";

    B.push_back(1, 1, 1.33);
    B.push_back(1, 2, 2.33);
    B.push_back(2, 1, 3.33);
    B.push_back(3, 1, 4.33);

    tout << "B(1, 2) = " << B(1, 2) << "\n";
    tout << "B[2][1] = " << B[2][1] << "\n";

    MTL_THROW_IF(std::abs(B(3, 1) - 4.33) > 0.001, unexpected_result());
    MTL_THROW_IF(std::abs(B(1, 2) - 2.33) > 0.001, unexpected_result());
    MTL_THROW_IF(std::abs(B(2, 1) - 2.33) > 3.001, unexpected_result());

    B.push_back(1, 0, 5.33);
    B.push_back(0, 0, 6.33);

    B.print_internal(tout);
    B.sort();

    tout << "Sorted\n";
    B.print_internal(tout);

    tout << "x=" << x << "\n" << "res=" << res << "\n";
    res = B * x ;
    tout << "res=" << res << "\n";

    res2= 6.33, 8.99, 3.33, 4.33, 0;
    res-= res;
    if (one_norm(res) > 0.1)
	throw "Matrix vector product wrong.";

    matrix_type A(5, 5, 9);
    {
	mat::inserter<matrix_type> ins(A, 3);
	ins[1][2] << 13.3;
	ins[2][2] << 23.3;
	ins[2][3] << 33.3;
	ins[2][4] << 33.3;
	ins[0][4] << 53.3;
	ins[1][4] << 6.0;
	ins[3][0] << 73.3;
    }
    tout << "A (internal) after first insertion\n";
    A.print_internal(tout);
    MTL_THROW_IF(std::abs(A[2][3] - 33.3) > 0.001, unexpected_result());
    MTL_THROW_IF(std::abs(A[2][1]) > 0.001, unexpected_result());

    {
	mat::inserter<matrix_type, operations::update_plus<double> > ins(A);
	ins[2][3] << 3.0;
	ins[2][1] << 3.33;
    }
    tout << "A (internal) after updating insertion\n";
    A.print_internal(tout);
    MTL_THROW_IF(std::abs(A[2][3] - 36.3) > 0.001, unexpected_result());
    MTL_THROW_IF(std::abs(A[2][1] - 3.33) > 0.001, unexpected_result());
    MTL_THROW_IF(std::abs(A[2][0]) > 0.001, unexpected_result());

    tout << "A (internal) =\n";
    A.print_internal(tout);

    tout << "A =\n" << A;

    traits::row<matrix_type>::type             row(A);
    traits::col<matrix_type>::type             col(A);
    traits::const_value<matrix_type>::type     value(A);

    typedef traits::range_generator<tag::major, matrix_type>::type  cursor_type;

    for (cursor_type cursor = mtl::begin<tag::major>(A), cend = mtl::end<tag::major>(A);
	 cursor != cend; ++cursor) {

	typedef traits::range_generator<tag::nz, cursor_type>::type icursor_type;
	for (icursor_type icursor = mtl::begin<tag::nz>(cursor), icend = mtl::end<tag::nz>(cursor);
	     icursor != icend; ++icursor)
	    tout << "A[" << row(*icursor) << "][" << col(*icursor) << "] = " << value(*icursor) << '\n';
    }

    mtl::mat::compressed2D<double> C(A);
    tout << "C=\n"<< C << "\n";

    return 0;
}


/*===================================================================

MSI applications for interactive analysis in MITK (M2aia)

Copyright (c) Jonas Cordes

All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt for details.

===================================================================*/

#include "mitkIOUtil.h"
#include <m2RunningMedian.h>
#include <m2TestingConfig.h>
#include <mitkTestFixture.h>
#include <mitkTestingMacros.h>
#include <numeric>
#include <algorithm>
#include <random>

//#include <boost/algorithm/string.hpp>

class m2RunningMedianTestSuite : public mitk::TestFixture
{
  CPPUNIT_TEST_SUITE(m2RunningMedianTestSuite);
  MITK_TEST(ApplyRunningMedian_SeededGaussianNoise_shouldReturnTrue);

  CPPUNIT_TEST_SUITE_END();

private:
public:
  void ApplyRunningMedian_SeededGaussianNoise_shouldReturnTrue()
  {
    /*const double mean = 1.0;
    const double stddev = 0.33;
    std::default_random_engine generator;
    generator.seed(142191);
    std::normal_distribution<double> dist(mean, stddev);*/

    std::vector<double> signal = {5, 5, 9, 5, 5, 5, 5, 0, 4, 4, 4, 6, 6, 6};
    std::vector<double> result = {5, 5, 5, 5, 5, 5, 5, 5, 4, 4, 4, 4, 6, 6};

    std::vector<double> median(signal.size());
    m2::RunMedian::apply(signal, 1, median); // half window size = 1; so window size is 2*1+1

	auto res = std::mismatch(std::begin(median), std::end(median), std::begin(result));

	CPPUNIT_ASSERT(res.first == std::end(median));

  }
};

MITK_TEST_SUITE_REGISTRATION(m2RunningMedian)


#include <benchmark/benchmark.h>

#include <boost/numeric/ublas/matrix.hpp>

#include <mitrax/dim.hpp>

#include <algorithm>

#include "../../../include/random_vector.hpp"

using namespace mitrax;
using namespace mitrax::literals;

template < typename T >
[[gnu::noinline]]
void bm(benchmark::State& state, rt_dim_pair_t d){
	auto r = mitrax::random_vector< T >(d.point_count());

	while(state.KeepRunning()){
		boost::numeric::ublas::matrix< T > m(
			size_t(d.cols()), size_t(d.rows())
		);

		std::copy(r.begin(), r.end(), m.data().begin());

		benchmark::DoNotOptimize(m);
	}
}

#include <boost/hana/tuple.hpp>
#include <boost/hana/for_each.hpp>

namespace init{

	constexpr auto dimensions = boost::hana::make_tuple(
			dim_pair(2_CS, 2_RS),
			dim_pair(4_CS, 2_RS),
			dim_pair(8_CS, 2_RS),
			dim_pair(8_CS, 4_RS),
			dim_pair(8_CS, 8_RS),
			dim_pair(8_CS, 16_RS),
			dim_pair(8_CS, 32_RS),
			dim_pair(8_CS, 64_RS),
			dim_pair(16_CS, 64_RS),
			dim_pair(32_CS, 64_RS),
			dim_pair(64_CS, 64_RS),
			dim_pair(128_CS, 64_RS),
			dim_pair(256_CS, 64_RS),
			dim_pair(256_CS, 128_RS),
			dim_pair(256_CS, 256_RS)
		);

}

#include "main.hpp"


#include <Eigen/Core>

Eigen::VectorXd make(const Eigen::VectorXd& vector)
{
    return 2.0 * vector;
}


#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <unistd.h>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "../trajectory.txt";
string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

// start point is red and end point is blue
void DrawTrajectory(TrajectoryType);
void DrawTwoTrajectory(TrajectoryType, TrajectoryType);
void CalculateError(TrajectoryType, TrajectoryType );

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

    TrajectoryType poses;
    TrajectoryType groundtruth;
    TrajectoryType estimated;

    /// implement pose reading code
    std::ifstream file_reader(trajectory_file);
    std::ifstream groundtruth_reader(groundtruth_file);
    std::ifstream estimated_reader(estimated_file);
    double time, tx, ty, tz, qx, qy, qz, qw;

    // Method 1
    // while (!file_reader.eof()){
    //     file_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
    //     Eigen::Vector3d t(tx, ty, tz);
    //     Eigen::Quaterniond q(qw, qx, qy, qz);
    //     q.normalize();
    //     Eigen::Matrix3d Rotation_Matrix(q);
    //     Sophus::SE3d SE3_from_Eigen(Rotation_Matrix, t);
    //     poses.push_back(SE3_from_Eigen);
    // }
    // cout << "There are " << poses.size() << " timestamps." << endl;

    // Method 2
    while(file_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw){
        poses.push_back(Sophus::SE3d(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)));
    }
    cout << "There are " << poses.size() << " timestamps." << endl;

    //    DrawTrajectory(poses);

    while(groundtruth_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw){
        groundtruth.push_back(Sophus::SE3d(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)));
    }
    cout << "There are " << groundtruth.size() << " timestamps." << endl;

    while(estimated_reader >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw){
        estimated.push_back(Sophus::SE3d(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz)));
    }
    cout << "There are " << estimated.size() << " timestamps." << endl;
    DrawTwoTrajectory(groundtruth, estimated);

    CalculateError(groundtruth, estimated);

    return 0;
}

void CalculateError(TrajectoryType groundtruth, TrajectoryType estimated){
    double error(0.0);
    double rmse(0.0);
    for (size_t i = 0; i < groundtruth.size(); i++){
        error = (groundtruth[i].inverse() * estimated[i]).log().norm();
        rmse += error * error;
    }
    rmse = sqrt(rmse / groundtruth.size());
    cout << "rmse: " << rmse << endl;
}

/*******************************************************************************************/
void DrawTrajectory(TrajectoryType poses) {

    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }

}

void DrawTwoTrajectory(TrajectoryType poses1, TrajectoryType poses2) {
    if (poses1.empty() || poses2.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::CreateWindowAndBind("Groundtruth: red, Estimated: blue", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);

    pangolin::OpenGlRenderState s_cam(
            pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
            pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );

    pangolin::View &d_cam = pangolin::CreateDisplay()
            .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
            .SetHandler(new pangolin::Handler3D(s_cam));

    while (pangolin::ShouldQuit() == false) {
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

        d_cam.Activate(s_cam);
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);

        glLineWidth(2);
        for (size_t i = 0; i < poses1.size() - 1; i++) {
            glColor3f(1.0f, 0.0f, 0.0f);
            glBegin(GL_LINES);
            auto p1 = poses1[i], p2 = poses1[i + 1];
            glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
            glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
            glEnd();

            glColor3f(0.0f, 0.0f, 1.0f);
            glBegin(GL_LINES);
            auto p3 = poses2[i], p4 = poses2[i + 1];
            glVertex3d(p3.translation()[0], p3.translation()[1], p3.translation()[2]);
            glVertex3d(p4.translation()[0], p4.translation()[1], p4.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}


#include "hsi_data_reader.h"
#include "spectral.h"
#include "settings.h"
#include "write_log_file.h"
#include <cmath>
#include <Eigen/Dense>
#include <chrono>
#include <filesystem>

#include "opencv2/highgui.hpp"
#include <vector>
#include <iostream>
#include <sstream>
#include <ctype.h>
#include <cstdlib>
#include <unordered_map>

using hsi::HSIData;
using hsi::HSIDataOptions;
using hsi::HSIDataReader;

float normalPDF(float s, float x, float m){
  return (1/(s*2.50663))*std::pow(2.71828,-0.5*std::pow((x-m)/s,2.0));
}

// convert a comma seperate sting list of the form
// [first, second, third, ... , last]
// to a vector of strings
std::vector<std::string> stringCS2Vector(std::string str){
  str.erase(std::remove(str.begin(), str.end(), '}'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '{'), str.end());
  str.erase(std::remove(str.begin(), str.end(), ']'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '['), str.end());
  std::vector<std::string> result;
  std::stringstream ss(str);
  while (ss.good()){
    std::string substr;
    std::getline(ss, substr, ',');
    result.push_back(substr);
  }
  return result;
}

// convert a comma seperate sting list of the form
// {first, second, third, ... , last}
// to a Eigen::VectorXf
Eigen::VectorXf stringCS2VectorFloats(std::string str, int len){
  Eigen::VectorXf result(len);
  str.erase(std::remove(str.begin(), str.end(), '}'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '{'), str.end());
  str.erase(std::remove(str.begin(), str.end(), ']'), str.end());
  str.erase(std::remove(str.begin(), str.end(), '['), str.end());
  std::stringstream ss(str);
  int i = 0;
  while (ss.good()){
    std::string substr;
    std::getline(ss, substr, ',');
    float num_float = std::stof(substr);
    result(i) = num_float;
    i++;
  }
  return result;
}

RGBwavelengthsStruct computeRGBwavelengths(Eigen::VectorXf wl){
  RGBwavelengthsStruct RGBwavelengths;
  float redDiffWL = abs(wl(0)-650);
  float greenDiffWL = abs(wl(0)-550);
  float blueDiffWL = abs(wl(0)-450);
  for (int band_idx = 0; band_idx < wl.size(); ++band_idx){
    if (abs(wl(band_idx)-650) < redDiffWL){
      RGBwavelengths.bandRed = band_idx;
    }
    if (abs(wl(band_idx)-550) < redDiffWL){
      RGBwavelengths.bandGreen = band_idx;
    }
    if (abs(wl(band_idx)-450) < redDiffWL){
      RGBwavelengths.bandBlue = band_idx;
    }
  }
  return RGBwavelengths;
}

std::string getDataFname(int argc, char** argv){
  // Set paths to image files.
  if (argc == 1) {
    // if there are no input file names then use the default image
    // AVIRS which should be located in the same directory as the executable file.
    return std::filesystem::path(argv[0]).replace_filename("AVIRIS").string();
  }else {
    // input image and header file names
    return argv[1];
  }
}

std::string getHeaderFname(int argc, char** argv){
  // Set paths to image files.
  if (argc == 1) {
    // if there are no input file names then use the default
    return std::filesystem::path(argv[0]).replace_filename("AVIRIS.hdr").string();;
  }else if (argc == 2) {
    // case where file name is image and header is just the image with .hdr added
    return strcat(argv[1],".hdr");
  } else {
    // input image and header file names
    return argv[2];
  }
}

// Get the range of data (lines, columns, bands) to read
// Currently read the whole image, but subsets could be read here
hsi::HSIDataRange getDataRange(hsi::HSIDataOptions data_options){
  // Set range of data we want to read.
  hsi::HSIDataRange data_range;
  data_range.start_row = 0;
  data_range.end_row = data_options.num_data_rows;
  data_range.start_col = 0;
  data_range.end_col = data_options.num_data_cols;
  data_range.start_band = 0;
  data_range.end_band = data_options.num_data_bands;
  return data_range;
}

ImData HSIData2EigenData(const hsi::HSIData& hsi_data, hsi::HSIDataOptions data_options, settingsValues settings){
  // Start timing
  auto t_start_HSIData2EigenData = std::chrono::high_resolution_clock::now();

  // Create the structure to hold the image and metadata
  ImData Im;

  Im.rows = hsi_data.num_rows;
  Im.cols = hsi_data.num_cols;
  Im.bands = hsi_data.num_bands;
  Eigen::MatrixXf im2d(Im.rows * Im.cols, Im.bands);
  for (int band = 0; band < Im.bands; ++band) {
    for (int row = 0; row < Im.rows; ++row) {
      for (int col = 0; col < Im.cols; ++col) {
        im2d(row * Im.cols + col, band) = hsi_data.GetValue(row, col, band).value_as_float;
      }
    }
  }
  Im.im2d = im2d;

  // This creates the Eigen::VectorXf Im.wl from the wavelength string
  Im.wl = stringCS2VectorFloats(data_options.wavelength, Im.bands);

  // Im.wlScale is a scale factor on the wavelengths
  // multiplying by this factor puts the wavelengths
  // into units of nanometers
  Im.wlScale = 1;
  if (Im.wl.mean() < 10){
    Im.wlScale = 1000;
  }

  // Compute the indices for the wavelengths associated with
  // Reg, Green, Blue colors (650nm, 550nm, 450nm)
  RGBwavelengthsStruct RGBwavelengths = computeRGBwavelengths(Im.wl);
  int bandRed = RGBwavelengths.bandRed;
  int bandGreen = RGBwavelengths.bandGreen;
  int bandBlue = RGBwavelengths.bandBlue;

  // This creates the Eigen::VectorXf Im.fwhm from the fwhm string
  Im.fwhm = stringCS2VectorFloats(data_options.fwhm, Im.bands);

  auto t_end_HSIData2EigenData = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_HSIData2EigenData - t_start_HSIData2EigenData);
  Logger("Converting HSIDATA to Eigen Matrices completed in : "+std::to_string(secondsElapsedWhite.count()/1000.)+"s", settings);

  std::cout << "some data from the image: \n" << im2d(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return Im;
}

void showRGBimage(ImData Im, settingsValues settings){
  //  create grayscale image
  Eigen::MatrixXf RX2d = Im.RX.reshaped(Im.rows, Im.cols);
  const cv::Size RX_image_size(RX2d.cols(), RX2d.rows());
  float min_value = 0;
  float max_value = 0;
  cv::Mat RX_image(RX_image_size, CV_64FC1);
  for (int row = 0; row < RX2d.rows(); ++row) {
    for (int col = 0; col < RX2d.cols(); ++col) {
      const float pixel_value = RX2d(row, col);
      RX_image.at<double>(row, col) = pixel_value;
      min_value = std::min(min_value, pixel_value);
      max_value = std::max(max_value, pixel_value);
    }
  }
  RX_image = 255*(RX_image-min_value)/(max_value-min_value);
  cv::namedWindow("test", cv::WINDOW_AUTOSIZE);
  cv::imshow("test", RX_image);
  cv::waitKey(0);
}

// Computes statistics for an image
ImStats computeImageStats(ImData Im, settingsValues settings) {
  // Create the structure to hold all the stats
  ImStats stats;

  // Compute the image mean
  stats.mean = Im.im2d.colwise().mean();

  // Compute the covariance matrix
  auto t_start_cov_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd centered = (Im.im2d.rowwise() - stats.mean.transpose()).template cast<double>();
  Eigen::MatrixXd cov = (centered.transpose() * centered) / float(Im.im2d.rows() - 1);
  auto t_end_cov_Eigin = std::chrono::high_resolution_clock::now();
  auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_cov_Eigin - t_start_cov_Eigin);
  Logger("Covariance computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
    settings);

  // Compute the eigenvalues and eigenvectors
  auto t_start_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(cov);
  auto t_end_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd evals;
  Eigen::MatrixXd evecs;
  if (eigensolver.info() == Eigen::Success){
    auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_evalsEvecs_Eigin - t_start_evalsEvecs_Eigin);
    Logger("Eigenvalue and eignvector computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
      settings);
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  else {
    Logger("WARNING: eigenvalues and eigenvectors failed to compute.", settings);
  };

  std::cout << "Eigenvalues:\n";
  for (int i = 0; i < 3; ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }
  std::cout << "...\n";
  for (int i = evals.rows()-3; i < evals.rows(); ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }

  // Computing the whitening matrix
  auto t_start_W = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd D = Eigen::MatrixXd::Identity(evals.rows(),evals.rows());
  double maxEval = evals.maxCoeff();
  for (int i = 0; i < evals.rows(); ++i) {
    // Optional regularization on eigenvalues
    //stats.evals(i) = std::max(stats.evals(i),maxEval*std::pow(10,-8));
    D(i,i) = 1/sqrt(evals(i));
  }
  Eigen::MatrixXd W =  (evecs*D);
  std::cout << "some data from the whitening matrix: \n" << W(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  auto t_end_W = std::chrono::high_resolution_clock::now();
  auto secondsElapsedW = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_W - t_start_W);
  Logger("Whitening matrix computation completed in "+std::to_string(secondsElapsedW.count()/1000.)+"s",
    settings);

  // Recast as float
  stats.cov = cov.template cast<float>();
  stats.evals = evals.template cast<float>();
  stats.evecs = evecs.template cast<float>();
  stats.W = W.template cast<float>();

  Logger("im2d Matrix is "+std::to_string(Im.im2d.rows())+"x"+std::to_string(Im.im2d.cols()),
    settings);
  Logger("Covariance is "+std::to_string(stats.cov.rows())+"x"+std::to_string(stats.cov.cols()),
    settings);
  Logger("Whitening Matrix is "+std::to_string(stats.W.rows())+"x"+std::to_string(stats.W.cols()),
    settings);

  // VALIDATION: If we want to validate the Whitening by comparison to the inverse
  //Eigen::MatrixXd Diff = cov.inverse() - (W)*(W.transpose());
  //std::cout << "some data from the Diff matrix: \n" << Diff(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return stats;
}

// Whitens an image using provided statistics
Eigen::MatrixXf computeWhitenedImage(ImData Im, ImStats stats, settingsValues settings) {

  auto t_start_White = std::chrono::high_resolution_clock::now();
  // VALIDATION: If we want to validat the data be viewing the image mean
  //std::cout << "image mean: \n" << stats.mean;
  Eigen::MatrixXf centered = Im.im2d.rowwise() - stats.mean.transpose();
  Eigen::MatrixXf whitened = centered*stats.W;
  auto t_end_White = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_White - t_start_White);
  Logger("Whitening image completed in "+std::to_string(secondsElapsedWhite.count()/1000.)+"s",
    settings);
  Logger("Whitening image is "+std::to_string(whitened.rows())+"x"+std::to_string(whitened.cols()),
    settings);

  return whitened;
}

// Computes the RX anomaly detetion image from the whitened image
Eigen::MatrixXf computeRXimage(ImData Im, settingsValues settings){

  auto t_start_rx = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf  RX = Im.white2d.rowwise().norm();
  Logger("RX image has size: "+std::to_string(RX.size()), settings);
  auto t_end_rx = std::chrono::high_resolution_clock::now();
  auto secondsElapsedRX = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_rx - t_start_rx);
  Logger("Creating RX image completed in "+std::to_string(secondsElapsedRX.count()/1000.)+"s", settings);

  return RX;
}

// Displays the RX anomaly detection image
void showRXimage(ImData Im, settingsValues settings){

  if (settings.saveRX || settings.displayRX){
    //  create grayscale image
    Eigen::MatrixXf RX2d = Im.RX.reshaped(Im.rows, Im.cols);
    const cv::Size RX_image_size(RX2d.cols(), RX2d.rows());
    float min_value = 0;
    float max_value = 0;
    cv::Mat RX_image(RX_image_size, CV_64FC1);
    for (int row = 0; row < RX2d.rows(); ++row) {
      for (int col = 0; col < RX2d.cols(); ++col) {
        const float pixel_value = RX2d(row, col);
        RX_image.at<double>(row, col) = pixel_value;
        min_value = std::min(min_value, pixel_value);
        max_value = std::max(max_value, pixel_value);
      }
    }
    RX_image = 5*(RX_image-min_value)/(max_value-min_value);

    if (settings.displayRX){
      cv::namedWindow("test", cv::WINDOW_AUTOSIZE);
      cv::imshow("test", RX_image);
      cv::waitKey(0);
    }

    if (settings.saveRX){
      bool check = cv::imwrite(settings.outDir+"\\RX.jpg", 255*RX_image);
      if (check == false) {
        Logger("WARNING Saving RX image failed.", settings);
      } else {
        Logger("RX Image saves as "+settings.outDir+"\\RX.jpg", settings);
      }
    }
  }
}

const std::string getTgtLibDataFname(const std::string hsi_data_path){
  std::filesystem::path p(hsi_data_path);
  return p.parent_path().string()+"\\lib_detect_fullresolution.sli";
}

const std::string getTgtLibHeaderFname(const std::string hsi_data_path){
  std::filesystem::path p(hsi_data_path);
  return p.parent_path().string()+"\\lib_detect_fullresolution.hdr";
}

// Converts data and metadata for a spectral library into
// a LibData structure using Eigen variable types.
LibData SpecLibData2EigenData(const hsi::HSIData& tgt_lib_data, hsi::HSIDataOptions tgt_lib_data_options){
  // Create the structure to hold the image and metadata
  LibData lib_tgt;
  lib_tgt.nSpectra = tgt_lib_data.num_cols;
  lib_tgt.bands = tgt_lib_data.num_rows;

  // This creates the vector Im.wl from the wavelength string
  lib_tgt.wl = stringCS2VectorFloats(tgt_lib_data_options.wavelength, lib_tgt.bands);

  // This creates the vector lib_tgt.specNames from the specNames string
  lib_tgt.specNames = stringCS2Vector(tgt_lib_data_options.specNames);

  // Make a matrix that will hold the spectra
  Eigen::MatrixXf spectra(lib_tgt.nSpectra, lib_tgt.bands);
  for (int i = 0; i < lib_tgt.nSpectra; ++i) {
    for (int j = 0; j < lib_tgt.bands; ++j) {
      spectra(i,j) = 10.0*tgt_lib_data.GetValueLib(j, i, 0).value_as_float;
    }
  }
  lib_tgt.spectra = spectra;
  std::cout << "some data from the library file spectra: \n" << spectra(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return lib_tgt;
}

LibData resampleLibrary(LibData lib_tgt_fullres, ImData Im) {
  // Create the structure to hold the image and metadata
  LibData lib_tgt;
  lib_tgt.specNames = lib_tgt_fullres.specNames;
  lib_tgt.nSpectra = lib_tgt_fullres.nSpectra;
  lib_tgt.bands = Im.bands;
  lib_tgt.wl = Im.wl;
  Eigen::MatrixXf spectra = Eigen::MatrixXf::Zero(lib_tgt_fullres.nSpectra, Im.bands);

  Eigen::VectorXf sigma(Im.bands);
  for (int i = 0; i < Im.bands; ++i) {
    sigma(i) = Im.fwhm(i)/2.355;
  }

  float sum = 0;
  float weight = 0;
  for (int im_bnd_idx = 0; im_bnd_idx < Im.bands; ++im_bnd_idx) {  //  looping over bands in the image
    for (int lb_bnd_idx = 0; lb_bnd_idx < lib_tgt_fullres.bands; ++lb_bnd_idx) { // looping over all bands in the library
      //  compute the difference between the image (target) wavelength and library wavelength so that we
      //  only compute contributions of library bands with wavelength that are withing 3 sigma of the image wavelength
      float diff = (lib_tgt_fullres.wl[lb_bnd_idx] - Im.wl[im_bnd_idx]);
      float standard_deviations_diff = diff/sigma(im_bnd_idx);

      /*  VALIDATION OUTPUT:  uncomment this to see how the resampling computation works
      std::cout << lib_tgt_fullres.wl[lb_bnd_idx] << " - " << Im.wl[im_bnd_idx] << std::endl;
      std::cout << diff << std::endl;
      std::cout << standard_deviations_diff << std::endl;
      std::cout << "Weight: " << weight << std::endl;
      std::cout << "Sum: " << sum << std::endl;
      */

      if (standard_deviations_diff > -3){
        if (standard_deviations_diff > 3){
          // stop the  iteration through library bands becasue the library band wavelength has
          // passed all wavelengths within 3 standard deviations of the image wavelength
          lb_bnd_idx = lib_tgt_fullres.bands;
        } else {
          weight = normalPDF(sigma(im_bnd_idx),
                            Im.wl[im_bnd_idx],
                            lib_tgt_fullres.wl[lb_bnd_idx]);
          for (int spec_idx = 0; spec_idx < lib_tgt.nSpectra; ++spec_idx) { // looping over all spectra in the library
            spectra(spec_idx, im_bnd_idx) = spectra(spec_idx, im_bnd_idx) +
                                        lib_tgt_fullres.spectra(spec_idx, lb_bnd_idx)*
                                        weight;
            sum = sum + weight;
          }
        }
      }
    }
    for (int spec_idx = 0; spec_idx < lib_tgt.nSpectra; ++spec_idx) { // looping over all spectra in the library
      spectra(spec_idx, im_bnd_idx) = spectra(spec_idx, im_bnd_idx)/sum;
    }
    sum = 0;
  }
  lib_tgt.spectra = spectra;

  /* VALIDATION: If we want to validate data by checking the first resampled spectrum from the library
  std::cout << "First spectrum:\n";
  for (int idx = 0; idx < Im.bands; ++idx) {
    std::cout << spectra(0,idx) << std::endl;
  }
  */

  std::cout << "some data from the resampled spectra: \n";
  std::cout << spectra(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return lib_tgt;
}

Eigen::MatrixXf computeWhitenedLibrary(LibData lib_tgt, ImStats stats, settingsValues settings) {

  auto t_start_White = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXf centered = lib_tgt.spectra.rowwise() - stats.mean.transpose();
  std::cout << "some data from the centered spectra: \n" << centered(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf spectraWhite = centered*stats.W;
  std::cout << "some data from the whitened spectra: \n" << spectraWhite(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  auto t_end_White = std::chrono::high_resolution_clock::now();
  auto secondsElapsedWhite = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_White - t_start_White);
  Logger("Whitening library completed in "+std::to_string(secondsElapsedWhite.count()/1000.)+"s",
    settings);
  Logger("Whitening library is "+std::to_string(spectraWhite.rows())+"x"+std::to_string(spectraWhite.cols()),
    settings);

  return spectraWhite;
}

Eigen::MatrixXf computeACEimage(ImData Im, LibData lib_tgt, settingsValues settings){

  auto t_start_ACE = std::chrono::high_resolution_clock::now();
  std::cout << "some data from the whitened image: \n" << Im.white2d(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  std::cout << "some data from the whitened library: \n" << lib_tgt.spectraWhite(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf  ACE_num = Im.white2d * lib_tgt.spectraWhite.transpose();
  std::cout << "some data from the ACE numerator: \n" << ACE_num(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf  ACE_denom = (Im.RX.replicate(1,lib_tgt.nSpectra).array() * lib_tgt.spectraWhite.transpose().colwise().norm().replicate(Im.rows*Im.cols, 1).array()).matrix();
  std::cout << "some data from the ACE denominator: \n" << ACE_denom(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Eigen::MatrixXf  ACE = (ACE_num.array() / ACE_denom.array()).matrix();
  std::cout << "some data from the ACE image: \n" << ACE(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;
  Logger("ACE image is "+std::to_string(ACE_num.rows())+"x"+std::to_string(ACE_num.cols()),
    settings);
  auto t_end_ACE = std::chrono::high_resolution_clock::now();
  auto secondsElapsedACE = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_ACE - t_start_ACE);
  Logger("Creating ACE image completed in "+std::to_string(secondsElapsedACE.count()/1000.)+"s", settings);

  return ACE;
}

void showACEResults(ImData Im, LibData lib_tgt, Eigen::MatrixXf ACE, settingsValues settings){

  if (settings.saveACE || settings.displayACE){
    // Create the openCV ACE image
    const cv::Size ACEimSize(Im.cols, Im.rows);
    cv::Mat ACEim(ACEimSize, CV_64FC1);

    for (int spec_idx = 0; spec_idx < lib_tgt.nSpectra; ++spec_idx) {
      Eigen::MatrixXf ACEslice2d = ACE(Eigen::all, spec_idx);
      Eigen::MatrixXf ACE2d = ACEslice2d.reshaped(Im.rows, Im.cols);
      //  create grayscale image
      float min_value = 100;
      float max_value = -100;
      for (int row = 0; row < Im.rows; ++row) {
        for (int col = 0; col < Im.cols; ++col) {
          float pixel_value = ACE2d(row, col);
          //pixel_value = ((pixel_value>0)-(pixel_value<0))*std::sqrt(std::abs(pixel_value));
          float min_thresh = 0.25;
          pixel_value = std::max(min_thresh,pixel_value);
          ACEim.at<double>(row, col) = pixel_value;
          min_value = std::min(min_value, pixel_value);
          max_value = std::max(max_value, pixel_value);
        }
      }
      ACEim = (ACEim-min_value)/(max_value-min_value);
      std::cout << lib_tgt.specNames[spec_idx] << std::endl;
      std::cout << "Min Value: " << min_value << std::endl;
      std::cout << "Max Value: " << max_value << std::endl;

      if (settings.displayACE){
        cv::namedWindow(lib_tgt.specNames[spec_idx], cv::WINDOW_AUTOSIZE);
        cv::imshow(lib_tgt.specNames[spec_idx], ACEim);
        cv::waitKey(0);
      }
      if (settings.saveACE){
        std::string specName = lib_tgt.specNames[spec_idx];
        specName.erase(std::remove(specName.begin(), specName.end(), ' '), specName.end());
        bool check = cv::imwrite(settings.outDir+"\\ACE_"+specName+".jpg", 255*ACEim);
        if (check == false) {
          Logger("WARNING Saving ACE image failed.", settings);
        } else {
          Logger("ACE Image saved as "+settings.outDir+"\\ACE_"+specName+".jpg", settings);
        }
      }
    }
  }

}

// Compute a covarnaince matrix and other stats for the input image
// using a 3 step process: compute stats and RX anomaly detection,
// remove the anomalies, and recompute stats from non-anomalous pixels.
// Input image matrix is pixels x bands
ImStats computeAnomalyCleanedImageStats(ImData Im, settingsValues settings) {

  // build a subseted compy of the image structure containing
  // only about 10,000 pixel spectra for quick approximate
  // first covariance computation
  ImData Im_sampled;
  Im_sampled.im2d = Im.im2d(Eigen::seq(0,Im.im2d.rows(),std::floor(Im.im2d.rows()/10000.)), Eigen::all);
  Im_sampled.rows = Im.rows;
  Im_sampled.cols = Im.cols;

  // compute the covariance for the subset of pixel spectra
  Logger("Computing statistics using Eigen (first pass)", settings);
  ImStats stats_sampled = computeImageStats(Im_sampled, settings);

  // Whiten the full image using the subset-computed covariance
  Logger("Whitening the image. (first pass)", settings);
  Im_sampled.white2d = computeWhitenedImage(Im, stats_sampled, settings);

  // Compute the RX anomaly image using the subset-computed covariance
  // This will be used to remove anomalies for the
  // primary covariance computation
  Logger("Compute the RX anomaly image. (first pass)", settings);
  Eigen::MatrixXf RX = computeRXimage(Im_sampled, settings);

  // compute a threshold for anomaly removeal
  // removing the most anomalous 5% of pixels
  auto t_start_rx_pctile = std::chrono::high_resolution_clock::now();
  std::vector<float> RXv;
  RXv.resize(RX.size());
  Eigen::VectorXf::Map(&RXv[0], RX.size()) = RX;
  std::sort(RXv.begin(), RXv.end());
  int idx_95pct = std::round(RX.size()*0.95);
  for (int i = 0; i < 100; ++i) {
    std::cout << RXv[i] << std::endl;
  }
  float thresh_95pct = RXv[idx_95pct];
  std::cout << "Thresh: " << thresh_95pct << std::endl;
  std::cout << "Max: " << RX.maxCoeff() << std::endl;
  auto t_end_rx_pctile = std::chrono::high_resolution_clock::now();
  auto secondsElapsedRX_pctile = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_rx_pctile - t_start_rx_pctile);
  Logger("Computing RX threshold completed in "+std::to_string(secondsElapsedRX_pctile.count()/1000.)+"s", settings);

  auto t_subsample_start = std::chrono::high_resolution_clock::now();
  Eigen::VectorXi is_selected = (RX.array() < thresh_95pct).cast<int>();
  Eigen::VectorXi is_selectedindices(is_selected.sum());
  int idx = 0;
  for (int i = 0; i < Im.rows; ++i) {
    if (is_selected(i) == 1){
      is_selectedindices(idx) = i;
      idx++;
    }
  }

  Im_sampled.im2d = Im.im2d(is_selectedindices,Eigen::all);
  std::cout << Im_sampled.im2d.rows() << std::endl;
  std::cout << Im_sampled.im2d.cols() << std::endl;
  auto t_subsample_end = std::chrono::high_resolution_clock::now();
  auto secondsElapsed_subsample = std::chrono::duration_cast<std::chrono::milliseconds>(t_subsample_end - t_subsample_start);
  Logger("time for subset computation: "+std::to_string(secondsElapsed_subsample.count()/1000.)+"s", settings);

  // HAVING TROUBLE SUBSETTING THE IMAGE TO COMPUTE STATS FROM THE NON-ANOMALUS PIXELS
  // I AM MULTIPLYING ANOMALIES BY ZER FOR COV COMPUTATION BUT ITS NOT WORKING!

  // Now we compute the stats for the data with anomalies removed
  // Create the structure to hold all the stats
  ImStats stats;

  // Compute the image mean
  stats.mean = Im_sampled.im2d.colwise().mean();

  // Compute the covariance matrix
  auto t_start_cov_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd centered = (Im_sampled.im2d.rowwise() - stats.mean.transpose()).template cast<double>();
  Eigen::MatrixXd cov = (centered.transpose() * centered) / float(is_selected.sum() - 1);
  auto t_end_cov_Eigin = std::chrono::high_resolution_clock::now();
  auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_cov_Eigin - t_start_cov_Eigin);
  Logger("Covariance computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
    settings);

  // Compute the eigenvalues and eigenvectors
  auto t_start_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigensolver(cov);
  auto t_end_evalsEvecs_Eigin = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd evals;
  Eigen::MatrixXd evecs;
  if (eigensolver.info() == Eigen::Success){
    auto secondsElapsed = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_evalsEvecs_Eigin - t_start_evalsEvecs_Eigin);
    Logger("Eigenvalue and eignvector computation completed in "+std::to_string(secondsElapsed.count()/1000.)+"s",
      settings);
    evals = eigensolver.eigenvalues();
    evecs = eigensolver.eigenvectors();
  }
  else {
    Logger("WARNING: eigenvalues and eigenvectors failed to compute.", settings);
  };

  std::cout << "Eigenvalues:\n";
  for (int i = 0; i < 3; ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }
  std::cout << "...\n";
  for (int i = evals.rows()-3; i < evals.rows(); ++i) {
    std::cout << evals(i) << " | " << evals(i)/evals(evals.rows()-1) << std::endl;
  }

  // Computing the whitening matrix
  auto t_start_W = std::chrono::high_resolution_clock::now();
  Eigen::MatrixXd D = Eigen::MatrixXd::Identity(evals.rows(),evals.rows());
  double maxEval = evals.maxCoeff();
  for (int i = 0; i < evals.rows(); ++i) {
    // Optional regularization on eigenvalues
    //evals(i) = std::max(evals(i),maxEval*std::pow(10,-6));
    D(i,i) = 1/sqrt(evals(i));
  }
  Eigen::MatrixXd W =  (evecs*D);
  std::cout << "some data from the whitening matrix: \n" << W(Eigen::seq(0,8),Eigen::seq(0,8)) << std::endl;

  auto t_end_W = std::chrono::high_resolution_clock::now();
  auto secondsElapsedW = std::chrono::duration_cast<std::chrono::milliseconds>(t_end_W - t_start_W);
  Logger("Whitening matrix computation completed in "+std::to_string(secondsElapsedW.count()/1000.)+"s",
    settings);

  // Recast as float
  stats.cov = cov.template cast<float>();
  stats.evals = evals.template cast<float>();
  stats.evecs = evecs.template cast<float>();
  stats.W = W.template cast<float>();

  Logger("im2d Matrix is "+std::to_string(Im.im2d.rows())+"x"+std::to_string(Im.im2d.cols()),
    settings);
  Logger("Covariance is "+std::to_string(stats.cov.rows())+"x"+std::to_string(stats.cov.cols()),
    settings);
  Logger("Whitening Matrix is "+std::to_string(stats.W.rows())+"x"+std::to_string(stats.W.cols()),
    settings);

  // VALIDATION: If we want to validate the Whitening by comparison to the inverse
  //Eigen::MatrixXd Diff = cov.inverse() - (W)*(W.transpose());
  //std::cout << "some data from the Diff matrix: \n" << Diff(Eigen::seq(0,5),Eigen::seq(0,5)) << std::endl;

  return stats;
}


#include <stan/math/prim/mat.hpp>
#include <stan/math/prim/scal.hpp>
#include <test/unit/math/prim/prob/vector_rng_test_helper.hpp>
#include <test/unit/math/prim/prob/util.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/math/distributions.hpp>
#include <gtest/gtest.h>
#include <vector>
#include <limits>

class InvGammaTestRig : public VectorRealRNGTestRig {
 public:
  InvGammaTestRig()
      : VectorRealRNGTestRig(10000, 10, {0.5, 1.0, 1.3, 2.0}, {1, 2, 3},
                             {-2.5, -1.7, -0.1, 0.0}, {-3, -2, -1, 0},
                             {0.1, 1.0, 1.7, 2.1}, {1, 2, 3, 4},
                             {-2.7, -1.5, -0.5, 0.0}, {-3, -2, -1, 0}) {}

  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2& beta, const T3&,
                        T_rng& rng) const {
    return stan::math::inv_gamma_rng(alpha, beta, rng);
  }

  std::vector<double> generate_quantiles(double alpha, double beta,
                                         double) const {
    std::vector<double> quantiles;
    double K = stan::math::round(2 * std::pow(N_, 0.4));
    boost::math::inverse_gamma_distribution<> dist(alpha, beta);

    for (int i = 1; i < K; ++i) {
      double frac = i / K;
      quantiles.push_back(quantile(dist, frac));
    }
    quantiles.push_back(std::numeric_limits<double>::max());

    return quantiles;
  }
};

TEST(ProbDistributionsInvGamma, errorCheck) {
  check_dist_throws_all_types(InvGammaTestRig());
}

TEST(ProbDistributionsInvGamma, distributionTest) {
  check_quantiles_real_real(InvGammaTestRig());
}

TEST(ProbDistributionsInvGamma, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::inv_gamma_rng(4.0, 3.0, rng));

  EXPECT_THROW(stan::math::inv_gamma_rng(-4.0, 3.0, rng), std::domain_error);
  EXPECT_THROW(stan::math::inv_gamma_rng(4.0, -3.0, rng), std::domain_error);
  EXPECT_THROW(
      stan::math::inv_gamma_rng(stan::math::positive_infinity(), 3.0, rng),
      std::domain_error);
  EXPECT_THROW(
      stan::math::inv_gamma_rng(4, stan::math::positive_infinity(), rng),
      std::domain_error);
}

TEST(ProbDistributionsInvGamma, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  int N = 10000;
  int K = stan::math::round(2 * std::pow(N, 0.4));

  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(stan::math::inv_gamma_rng(2.0, 1.0, rng));
  }

  // Generate quantiles from boost's Inverse Gamma distribution
  boost::math::inverse_gamma_distribution<> dist(2.0, 1.0);
  std::vector<double> quantiles;
  for (int i = 1; i < K; ++i) {
    double frac = static_cast<double>(i) / K;
    quantiles.push_back(quantile(dist, frac));
  }
  quantiles.push_back(std::numeric_limits<double>::max());

  // Assert that they match
  assert_matches_quantiles(samples, quantiles, 1e-6);
}


//==================================================================================================
/*!

  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#include <boost/simd/function/scalar/stirling.hpp>
#include <scalar_test.hpp>
#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/two.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/function/round.hpp>

STF_CASE_TPL(" stirling",  STF_IEEE_TYPES)
{
  namespace bs = boost::simd;
  namespace bd = boost::dispatch;
  using bs::stirling;

  STF_EXPR_IS(stirling(T()),T);

#ifndef BOOST_SIMD_NO_INVALIDS
  STF_ULP_EQUAL(stirling(bs::Inf<T>()),  bs::Inf<T>(), 0.5);
  STF_ULP_EQUAL(stirling(bs::Minf<T>()), bs::Nan<T>(), 0.5);
  STF_ULP_EQUAL(stirling(bs::Mone<T>()), bs::Nan<T>(), 0.5);
  STF_ULP_EQUAL(stirling(bs::Nan<T>()),  bs::Nan<T>(), 0.5);
#endif
  STF_ULP_EQUAL(bs::round(stirling(bs::One<T>())),  bs::One<T>(), 0.5);
  STF_ULP_EQUAL(bs::round(stirling(bs::Two<T>())),  bs::One<T>(), 0.5);
}


///////////////////////////////////////////////////////////////////////////////
// statistics::model::algorithm::log_likelihood.hpp                          //
//                                                                           //
//  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)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_MODEL_ALGORITHM_LOG_LIKELIHOOD_HPP_ER_2009
#define BOOST_STATISTICS_MODEL_ALGORITHM_LOG_LIKELIHOOD_HPP_ER_2009
#include <boost/binary_op/algorithm/for_each.hpp>
#include <boost/statistics/model/wrap/aggregate/model_parameter.hpp>
#include <boost/statistics/model/functional/log_likelihood_accumulator.hpp>

namespace boost{
namespace statistics{
namespace model{

    // Evaluates the likelihood at mp.parameter(), given model mp.model(),
    // by summing all contributions from a dataset i.e. a
    // sequence of covariates, [b_x,e_x), and reponses (starting at b_y)
    template<typename T,typename M,typename P,typename ItX,typename ItE>
    T
    log_likelihood(
        model_parameter_<M,P> mp,
        ItX b_x,
        ItX e_x,
        ItE b_y
    ){
        typedef log_likelihood_accumulator<T,M,P> acc_;
        acc_ acc(mp);
        return boost::binary_op::for_each<acc_>(
            b_x,
            e_x,
            b_y,
            acc
        ).value();
    }

}// model
}// statistics
}// boost

#endif