//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE test_kernel

#include <iostream>

#include <boost/test/unit_test.hpp>

#include <limbo/kernel/exp.hpp>
#include <limbo/kernel/matern_five_halves.hpp>
#include <limbo/kernel/matern_three_halves.hpp>
#include <limbo/kernel/squared_exp_ard.hpp>
#include <limbo/tools/macros.hpp>
#include <limbo/tools/random_generator.hpp>

using namespace limbo;
struct Params {
    struct kernel : public defaults::kernel {
        BO_PARAM(double, noise, 0.0);
    };

    struct kernel_squared_exp_ard {
        BO_DYN_PARAM(int, k);
        BO_PARAM(double, sigma_sq, 1);
    };

    struct kernel_exp : public defaults::kernel_exp {
    };

    struct kernel_maternthreehalves : public defaults::kernel_maternthreehalves {
    };

    struct kernel_maternfivehalves : public defaults::kernel_maternfivehalves {
    };
};

struct ParamsNoise {
    struct kernel : public defaults::kernel {
        BO_PARAM(double, noise, 0.01);
        BO_PARAM(bool, optimize_noise, true);
    };

    struct kernel_squared_exp_ard {
        BO_PARAM(int, k, 0);
        BO_PARAM(double, sigma_sq, 1);
    };

    struct kernel_exp : public defaults::kernel_exp {
    };

    struct kernel_maternthreehalves : public defaults::kernel_maternthreehalves {
    };

    struct kernel_maternfivehalves : public defaults::kernel_maternfivehalves {
    };
};

BO_DECLARE_DYN_PARAM(int, Params::kernel_squared_exp_ard, k);

Eigen::VectorXd make_v2(double x1, double x2)
{
    Eigen::VectorXd v2(2);
    v2 << x1, x2;
    return v2;
}

// Check gradient via finite differences method
template <typename Kernel>
std::tuple<double, Eigen::VectorXd, Eigen::VectorXd> check_grad(const Kernel& kern, const Eigen::VectorXd& x, const Eigen::VectorXd& x1, const Eigen::VectorXd& x2, double e = 1e-4)
{
    Eigen::VectorXd analytic_result, finite_diff_result;
    Kernel ke = kern;
    ke.set_h_params(x);

    analytic_result = ke.grad(x1, x2);

    finite_diff_result = Eigen::VectorXd::Zero(x.size());
    for (int j = 0; j < x.size(); j++) {
        Eigen::VectorXd test1 = x, test2 = x;
        test1[j] -= e;
        test2[j] += e;
        Kernel k1 = kern;
        k1.set_h_params(test1);
        Kernel k2 = kern;
        k2.set_h_params(test2);
        double res1 = k1(x1, x2);
        double res2 = k2(x1, x2);
        finite_diff_result[j] = (res2 - res1) / (2.0 * e);
    }

    return std::make_tuple((analytic_result - finite_diff_result).norm(), analytic_result, finite_diff_result);
}

template <typename Kernel>
void check_kernel(size_t N, size_t K, double e = 1e-6)
{
    Kernel kern(N);

    for (size_t i = 0; i < K; i++) {
        Eigen::VectorXd hp = tools::random_vector(kern.h_params_size()).array() * 6. - 3.;

        double error;
        Eigen::VectorXd analytic, finite_diff;

        Eigen::VectorXd x1 = tools::random_vector(N).array() * 10. - 5.;
        Eigen::VectorXd x2 = tools::random_vector(N).array() * 10. - 5.;

        std::tie(error, analytic, finite_diff) = check_grad(kern, hp, x1, x2, e);
        // std::cout << error << ": " << analytic.transpose() << " vs " << finite_diff.transpose() << std::endl;
        BOOST_CHECK(error < 1e-5);
    }
}

BOOST_AUTO_TEST_CASE(test_grad_exp)
{
    for (int i = 1; i <= 10; i++) {
        check_kernel<kernel::Exp<Params>>(i, 100);
        check_kernel<kernel::Exp<ParamsNoise>>(i, 100);
    }
}

BOOST_AUTO_TEST_CASE(test_grad_matern_three)
{
    for (int i = 1; i <= 10; i++) {
        check_kernel<kernel::MaternThreeHalves<Params>>(i, 100);
        check_kernel<kernel::MaternThreeHalves<ParamsNoise>>(i, 100);
    }
}

BOOST_AUTO_TEST_CASE(test_grad_matern_five)
{
    for (int i = 1; i <= 10; i++) {
        check_kernel<kernel::MaternFiveHalves<Params>>(i, 100);
        check_kernel<kernel::MaternFiveHalves<ParamsNoise>>(i, 100);
    }
}

BOOST_AUTO_TEST_CASE(test_grad_SE_ARD)
{
    Params::kernel_squared_exp_ard::set_k(0);
    for (int i = 1; i <= 10; i++) {
        check_kernel<kernel::SquaredExpARD<Params>>(i, 100);
        check_kernel<kernel::SquaredExpARD<ParamsNoise>>(i, 100);
    }

    Params::kernel_squared_exp_ard::set_k(1);
    for (int i = 1; i <= 10; i++) {
        check_kernel<kernel::SquaredExpARD<Params>>(i, 100);
    }
}

BOOST_AUTO_TEST_CASE(test_kernel_SE_ARD)
{
    Params::kernel_squared_exp_ard::set_k(0);

    kernel::SquaredExpARD<Params> se(2);
    Eigen::VectorXd hp = Eigen::VectorXd::Zero(se.h_params_size());

    se.set_h_params(hp);

    Eigen::VectorXd v1 = make_v2(1, 1);
    BOOST_CHECK(std::abs(se(v1, v1) - 1) < 1e-6);

    Eigen::VectorXd v2 = make_v2(0, 1);
    double s1 = se(v1, v2);

    BOOST_CHECK(std::abs(s1 - std::exp(-0.5 * (v1.transpose() * v2)[0])) < 1e-5);

    hp(0) = 1;
    se.set_h_params(hp);
    double s2 = se(v1, v2);
    BOOST_CHECK(s1 < s2);

    Params::kernel_squared_exp_ard::set_k(1);
    se = kernel::SquaredExpARD<Params>(2);
    hp = Eigen::VectorXd::Zero(se.h_params_size());

    se.set_h_params(hp);
    BOOST_CHECK(s1 == se(v1, v2));
}


// Boost.Geometry (aka GGL, Generic Geometry Library)

// Copyright (c) 2007-2011 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2011 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2011 Mateusz Loskot, London, UK.

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

#include <iostream>

#include <boost/geometry/algorithms/distance.hpp>
#include <boost/geometry/algorithms/make.hpp>
#include <boost/geometry/geometries/register/point.hpp>
#include <boost/geometry/strategies/strategies.hpp>
#include <boost/geometry/util/write_dsv.hpp>

// Sample point, defining three color values
struct my_color_point
{
    double red, green, blue;
};

// Sample point, having an int array defined
struct my_array_point
{
    int c[3];
};

// Sample point, having x/y
struct my_2d
{
    float x,y;
};

// Sample class, protected and construction-time-only x/y,
// Can (of course) only used in algorithms which take const& points
class my_class_ro
{
public:
    my_class_ro(double x, double y) : m_x(x), m_y(y) {}
    double x() const { return m_x; }
    double y() const { return m_y; }
private:
    double m_x, m_y;
};

// Sample class using references for read/write
class my_class_rw
{
public:
    const double& x() const { return m_x; }
    const double& y() const { return m_y; }
    double& x() { return m_x; }
    double& y() { return m_y; }
private:
    double m_x, m_y;
};

// Sample class using getters / setters
class my_class_gs
{
public:
    const double get_x() const { return m_x; }
    const double get_y() const { return m_y; }
    void set_x(double v) { m_x = v; }
    void set_y(double v) { m_y = v; }
private:
    double m_x, m_y;
};

// Sample point within a namespace
namespace my
{
    struct my_namespaced_point
    {
        double x, y;
    };
}

BOOST_GEOMETRY_REGISTER_POINT_3D(my_color_point, double, cs::cartesian, red, green, blue)
BOOST_GEOMETRY_REGISTER_POINT_3D(my_array_point, int, cs::cartesian, c[0], c[1], c[2])
BOOST_GEOMETRY_REGISTER_POINT_2D(my_2d, float, cs::cartesian, x, y)
BOOST_GEOMETRY_REGISTER_POINT_2D_CONST(my_class_ro, double, cs::cartesian, x(), y())
BOOST_GEOMETRY_REGISTER_POINT_2D(my_class_rw, double, cs::cartesian, x(), y())
BOOST_GEOMETRY_REGISTER_POINT_2D_GET_SET(my_class_gs, double, cs::cartesian, get_x, get_y, set_x, set_y)

BOOST_GEOMETRY_REGISTER_POINT_2D(my::my_namespaced_point, double, cs::cartesian, x, y)

int main()
{
    // Create 2 instances of our custom color point
    my_color_point c1 = boost::geometry::make<my_color_point>(255, 3, 233);
    my_color_point c2 = boost::geometry::make<my_color_point>(0, 50, 200);

    // The distance between them can be calculated using the cartesian method (=pythagoras)
    // provided with the library, configured by the coordinate_system type of the point
    std::cout << "color distance "
        << boost::geometry::dsv(c1) << " to "
        << boost::geometry::dsv(c2) << " is "
        << boost::geometry::distance(c1,c2) << std::endl;

    my_array_point a1 = {{0}};
    my_array_point a2 = {{0}};
    boost::geometry::assign_values(a1, 1, 2, 3);
    boost::geometry::assign_values(a2, 3, 2, 1);

    std::cout << "color distance "
        << boost::geometry::dsv(a1) << " to "
        << boost::geometry::dsv(a2) << " is "
        << boost::geometry::distance(a1,a2) << std::endl;

    my_2d p1 = {1, 5};
    my_2d p2 = {3, 4};
    std::cout << "float distance "
        << boost::geometry::dsv(p1) << " to "
        << boost::geometry::dsv(p2) << " is "
        << boost::geometry::distance(p1,p2) << std::endl;

    my_class_ro cro1(1, 2);
    my_class_ro cro2(3, 4);
    std::cout << "class ro distance "
        << boost::geometry::dsv(cro1) << " to "
        << boost::geometry::dsv(cro2) << " is "
        << boost::geometry::distance(cro1,cro2) << std::endl;

    my_class_rw crw1;
    my_class_rw crw2;
    boost::geometry::assign_values(crw1, 1, 2);
    boost::geometry::assign_values(crw2, 3, 4);
    std::cout << "class r/w distance "
        << boost::geometry::dsv(crw1) << " to "
        << boost::geometry::dsv(crw2) << " is "
        << boost::geometry::distance(crw1,crw2) << std::endl;

    my_class_gs cgs1;
    my_class_gs cgs2;
    boost::geometry::assign_values(cgs1, 1, 2);
    boost::geometry::assign_values(cgs2, 3, 4);
    std::cout << "class g/s distance "
        << boost::geometry::dsv(crw1) << " to "
        << boost::geometry::dsv(crw2) << " is "
        << boost::geometry::distance(cgs1,cgs2) << std::endl;

    my::my_namespaced_point nsp1 = boost::geometry::make<my::my_namespaced_point>(1, 2);
    my::my_namespaced_point nsp2 = boost::geometry::make<my::my_namespaced_point>(3, 4);
    std::cout << "namespaced distance "
        << boost::geometry::dsv(nsp1) << " to "
        << boost::geometry::dsv(nsp2) << " is "
        << boost::geometry::distance(nsp1,nsp2) << std::endl;

    return 0;
}


// Accumulate even integers
#include <cassert>
#include <iostream>
#include <limits>
#include <memory>
#include <numeric>
#include <vector>
#include <boost/iterator/iterator_facade.hpp>
#include <boost/multiprecision/cpp_int.hpp>
#include <gtest/gtest.h>

namespace {
    // Expect RangePtr is a std::shared_ptr<> type
    template <typename Value, typename RangePtr>
    class MyRangeIterator : public boost::iterator_facade<
        MyRangeIterator<Value, RangePtr>,
        Value, boost::forward_traversal_tag, Value> {
    public:
        MyRangeIterator(void);
        explicit MyRangeIterator(RangePtr& pRange);
    private:
        friend class boost::iterator_core_access;
        void increment(void);
        Value dereference(void) const;
        bool equal(const MyRangeIterator& other) const;
        void update(void);
        RangePtr pRange_;
        Value current_ {0};
        Value value_ {RangePtr::element_type::OutOfBound};
    };

    template <typename T>
    class MyRange {
    public:
        using Value = T;
        using Iterator = MyRangeIterator<T, MyRange>;
        static const T OutOfBound;

        MyRange(const T& limit) : limit_(limit) {
            assert((OutOfBound == 0) || (limit < OutOfBound));
        }

        MyRange(void) {
            return;
        }

        const T& GetLimit(void) const {
            return limit_;
        }

        T GetValue(const T& current) const {
            if (current <= 0) {
                return OutOfBound;
            }

            const auto value = limit_ - current;
            const auto result = (value % Value{2}) ? Value{0} : value;
            return result;
        }

    private:
        const T limit_ {0};
    };

    template <typename T>
    const T MyRange<T>::OutOfBound = std::numeric_limits<T>::max();

    template <typename Value, typename RangePtr>
    MyRangeIterator<Value, RangePtr>::MyRangeIterator(void) {
        update();
        return;
    }

    template <typename Value, typename RangePtr>
    MyRangeIterator<Value, RangePtr>::MyRangeIterator(RangePtr& pRange) :
        pRange_(pRange), current_(pRange->GetLimit()),
        value_(pRange_->GetValue(current_)) {
        update();
        return;
    }

    template <typename Value, typename RangePtr>
    void MyRangeIterator<Value, RangePtr>::increment(void) {
        if (current_ > 0) {
            --current_;
        }
        update();
        return;
    }

    template <typename Value, typename RangePtr>
    Value MyRangeIterator<Value, RangePtr>::dereference(void) const {
        // Cannot return member variables as Value& from const member functions
        return value_;
    }

    template <typename Value, typename RangePtr>
    bool MyRangeIterator<Value, RangePtr>::equal(const MyRangeIterator& other) const {
        return (!pRange_ && !other.pRange_) || (pRange_ && other.pRange_ && (current_ == other.current_));
    }

    template <typename Value, typename RangePtr>
    void MyRangeIterator<Value, RangePtr>::update(void) {
        if (current_ <= 0) {
            pRange_.reset();
        }

        value_ = (pRange_) ? pRange_->GetValue(current_) : RangePtr::element_type::OutOfBound;
        return;
    }
}

template <typename RangePtr>
auto begin(RangePtr& pRange) {
    return MyRangeIterator<typename RangePtr::element_type::Value, RangePtr>(pRange);
}

template <typename RangePtr>
auto end(RangePtr& pRange) {
    return MyRangeIterator<typename RangePtr::element_type::Value, RangePtr>();
}

class TestRangeStream : public ::testing::Test {};

TEST_F(TestRangeStream, RangeInt) {
    using Value = int;
    const std::vector<Value> testedSet {1,2,3,6,8,9};
    for(const auto& limit : testedSet) {
        MyRange<Value> range(limit);

        for (Value i=1; i<limit; ++i) {
            EXPECT_EQ(limit, range.GetLimit());
            const auto diff = limit - i;
            const auto expected = ((diff % 2) == 0) ? diff : 0;
            EXPECT_EQ(expected, range.GetValue(i));
        }

        const auto expected = std::numeric_limits<Value>::max();
        EXPECT_EQ(expected, range.GetValue(0));
        EXPECT_EQ(expected, range.GetValue(-1));
    }
}

TEST_F(TestRangeStream, RangeEmpty) {
    using Value = int;
    MyRange<Value> rangeZero(0);
    EXPECT_FALSE(rangeZero.GetLimit());
    EXPECT_EQ(std::numeric_limits<Value>::max(), rangeZero.GetValue(0));

    MyRange<Value> rangeNegative(-1);
    EXPECT_EQ(-1, rangeNegative.GetLimit());
    EXPECT_EQ(std::numeric_limits<Value>::max(), rangeNegative.GetValue(0));
}

TEST_F(TestRangeStream, RangeBigInt) {
    using Value = boost::multiprecision::cpp_int;
    std::vector<Value> testedSet {1,15,16};
    for(const auto& limit : testedSet) {
        MyRange<Value> range(limit);
        for (Value i=1; i<limit; ++i) {
            EXPECT_EQ(limit, range.GetLimit());
            const auto diff = limit - i;
            const auto expected = ((diff % Value{2}) == Value{0}) ? diff : Value{0};
            EXPECT_EQ(expected, range.GetValue(i));
        }

        EXPECT_EQ(0, range.GetValue(0));
        EXPECT_EQ(0, range.GetValue(-1));
    }
}

TEST_F(TestRangeStream, Iterators) {
    using Value = int;
    constexpr Value length = 5;
    auto range = std::make_shared<MyRange<Value>>(length);

    auto ia = begin(range);
    auto ib = begin(range);
    auto ie = end(range);
    EXPECT_FALSE(*ia);
    EXPECT_FALSE(*ib);
    EXPECT_EQ(ia, ib);
    EXPECT_EQ(ib, ia);
    EXPECT_NE(ie, ia);
    EXPECT_NE(ie, ib);

    ++ia;
    EXPECT_FALSE(*ia);
    EXPECT_FALSE(*ib);
    EXPECT_NE(ia, ib);
    EXPECT_NE(ib, ia);
    EXPECT_NE(ie, ia);
    EXPECT_NE(ie, ib);

    ++ia;
    EXPECT_EQ(2, *ia);
    EXPECT_FALSE(*ib);
    EXPECT_NE(ia, ib);
    EXPECT_NE(ib, ia);
    EXPECT_NE(ie, ia);
    EXPECT_NE(ie, ib);

    ++ia;
    ++ia;
    EXPECT_EQ(4, *ia);
    EXPECT_FALSE(*ib);
    EXPECT_NE(ia, ib);
    EXPECT_NE(ib, ia);
    EXPECT_NE(ie, ia);
    EXPECT_NE(ie, ib);

    ++ib;
    ++ib;
    EXPECT_EQ(4, *ia);
    EXPECT_EQ(2, *ib);
    EXPECT_NE(ia, ib);
    EXPECT_NE(ib, ia);
    EXPECT_NE(ie, ia);
    EXPECT_NE(ie, ib);

    ++ib;
    ++ib;
    EXPECT_EQ(ia, ib);
    EXPECT_EQ(ib, ia);
    EXPECT_NE(ie, ia);
    EXPECT_NE(ie, ib);

    ++ia;
    EXPECT_NE(ia, ib);
    EXPECT_NE(ib, ia);
    const auto expected = std::numeric_limits<Value>::max();
    EXPECT_EQ(expected, *ia);
    EXPECT_EQ(ie, ia);
    EXPECT_NE(ie, ib);

    ++ib;
    EXPECT_EQ(ia, ib);
    EXPECT_EQ(ib, ia);
    EXPECT_EQ(ie, ia);
    EXPECT_EQ(ie, ib);
    EXPECT_EQ(expected, *ib);

    ++ia;
    ++ib;
    EXPECT_EQ(ia, ib);
    EXPECT_EQ(ib, ia);
    EXPECT_EQ(ie, ia);
    EXPECT_EQ(ie, ib);
    EXPECT_EQ(expected, *ia);
    EXPECT_EQ(expected, *ib);
}

TEST_F(TestRangeStream, IteratorEmpty) {
    using Value = int;
    auto range = std::make_shared<MyRange<Value>>(0);
    auto ia = begin(range);
    auto ie = end(range);
    EXPECT_EQ(ia, ie);

    const auto expected = std::numeric_limits<Value>::max();
    EXPECT_EQ(expected, *ia);
    EXPECT_EQ(ia, ie);
}

TEST_F(TestRangeStream, Member) {
    using Value = int;
    std::vector<Value> testedSet {1,2,9,10};
    for(const auto& length : testedSet) {
        auto range = std::make_shared<MyRange<Value>>(length);
        std::vector<Value> actual;

        auto it = begin(range);
        while(it != end(range)) {
            const auto value = *it;
            actual.push_back(value);
            ++it;
        }

        ASSERT_EQ(length, actual.size());
        for(Value i = 0; i < length; ++i) {
            if ((i % 2) == 0) {
                EXPECT_EQ(i, actual.at(i));
            } else {
                EXPECT_FALSE(actual.at(i));
            }
        }
    }
}

TEST_F(TestRangeStream, IntSum) {
    using Value = int;
    auto range = std::make_shared<MyRange<Value>>(101);
    const auto actual = std::accumulate(begin(range), end(range), Value{0},
                                  [](const auto& acc, const auto& element) {
                                      return acc + element;
                                  });
    EXPECT_EQ(2550, actual);
}

TEST_F(TestRangeStream, LongLongSum) {
    using Value = long long;
    auto range = std::make_shared<MyRange<Value>>(1000001);
    const auto actual = std::accumulate(begin(range), end(range), Value{0},
                                        [](const auto& acc, const auto& element) {
                                            return acc + element;
                                        });
    const Value expected = 250000500000ll;
    EXPECT_EQ(expected, actual);
}

TEST_F(TestRangeStream, BigInt) {
    using Value = boost::multiprecision::cpp_int;
    auto range = std::make_shared<MyRange<Value>>(1000001);
    const auto actual = std::accumulate(begin(range), end(range), Value{0},
                                        [](const auto& acc, const auto& element) {
                                            return acc + element;
                                        });
    const Value expected = 250000500000ll;
    EXPECT_EQ(expected, actual);
}

int main(int argc, char* argv[]) {
    ::testing::InitGoogleTest(&argc, argv);
    return RUN_ALL_TESTS();
}

/*
Local Variables:
mode: c++
coding: utf-8-dos
tab-width: nil
c-file-style: "stroustrup"
End:
*/


#ifndef INC_MUSPELHEIM_MUSPELHEIM_HPP
#define INC_MUSPELHEIM_MUSPELHEIM_HPP

#include "colors.hpp"
#include "ifs.hpp"
#include "variations.hpp"

#include <boost/gil/typedefs.hpp>

namespace muspelheim {

  using rgb8 = boost::gil::rgb8_pixel_t;
  using flame_function_system = ifs::iterated_function_system<rgb8>;

  extern flame_function_system function_system;

} // namespace muspelheim

#endif


// Copyright (c) 2018, ETH Zurich and UNC Chapel Hill.
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//
//     * Neither the name of ETH Zurich and UNC Chapel Hill nor the names of
//       its contributors may be used to endorse or promote products derived
//       from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: Johannes L. Schoenberger (jsch at inf.ethz.ch)

#include "base/polynomial.h"

#include <Eigen/Eigenvalues>

#include "util/logging.h"

namespace colmap {
namespace {

// Remove leading zero coefficients.
Eigen::VectorXd RemoveLeadingZeros(const Eigen::VectorXd& coeffs) {
  Eigen::VectorXd::Index num_zeros = 0;
  for (; num_zeros < coeffs.size(); ++num_zeros) {
    if (coeffs(num_zeros) != 0) {
      break;
    }
  }
  return coeffs.tail(coeffs.size() - num_zeros);
}

// Remove trailing zero coefficients.
Eigen::VectorXd RemoveTrailingZeros(const Eigen::VectorXd& coeffs) {
  Eigen::VectorXd::Index num_zeros = 0;
  for (; num_zeros < coeffs.size(); ++num_zeros) {
    if (coeffs(coeffs.size() - 1 - num_zeros) != 0) {
      break;
    }
  }
  return coeffs.head(coeffs.size() - num_zeros);
}

}  // namespace

bool FindLinearPolynomialRoots(const Eigen::VectorXd& coeffs,
                               Eigen::VectorXd* real, Eigen::VectorXd* imag) {
  CHECK_EQ(coeffs.size(), 2);

  if (coeffs(0) == 0) {
    return false;
  }

  if (real != nullptr) {
    real->resize(1);
    (*real)(0) = -coeffs(1) / coeffs(0);
  }

  if (imag != nullptr) {
    imag->resize(1);
    (*imag)(0) = 0;
  }

  return true;
}

bool FindQuadraticPolynomialRoots(const Eigen::VectorXd& coeffs,
                                  Eigen::VectorXd* real,
                                  Eigen::VectorXd* imag) {
  CHECK_EQ(coeffs.size(), 3);

  const double a = coeffs(0);
  if (a == 0) {
    return FindLinearPolynomialRoots(coeffs.tail(2), real, imag);
  }

  const double b = coeffs(1);
  const double c = coeffs(2);
  if (b == 0 && c == 0) {
    if (real != nullptr) {
      real->resize(1);
      (*real)(0) = 0;
    }
    if (imag != nullptr) {
      imag->resize(1);
      (*imag)(0) = 0;
    }
    return true;
  }

  const double d = b * b - 4 * a * c;

  if (d >= 0) {
    const double sqrt_d = std::sqrt(d);
    if (real != nullptr) {
      real->resize(2);
      if (b >= 0) {
        (*real)(0) = (-b - sqrt_d) / (2 * a);
        (*real)(1) = (2 * c) / (-b - sqrt_d);
      } else {
        (*real)(0) = (2 * c) / (-b + sqrt_d);
        (*real)(1) = (-b + sqrt_d) / (2 * a);
      }
    }
    if (imag != nullptr) {
      imag->resize(2);
      imag->setZero();
    }
  } else {
    if (real != nullptr) {
      real->resize(2);
      real->setConstant(-b / (2 * a));
    }
    if (imag != nullptr) {
      imag->resize(2);
      (*imag)(0) = std::sqrt(-d) / (2 * a);
      (*imag)(1) = -(*imag)(0);
    }
  }

  return true;
}

bool FindPolynomialRootsDurandKerner(const Eigen::VectorXd& coeffs_all,
                                     Eigen::VectorXd* real,
                                     Eigen::VectorXd* imag) {
  CHECK_GE(coeffs_all.size(), 2);

  const Eigen::VectorXd coeffs = RemoveLeadingZeros(coeffs_all);

  const int degree = coeffs.size() - 1;

  if (degree <= 0) {
    return false;
  } else if (degree == 1) {
    return FindLinearPolynomialRoots(coeffs, real, imag);
  } else if (degree == 2) {
    return FindQuadraticPolynomialRoots(coeffs, real, imag);
  }

  // Initialize roots.
  Eigen::VectorXcd roots(degree);
  roots(degree - 1) = std::complex<double>(1, 0);
  for (int i = degree - 2; i >= 0; --i) {
    roots(i) = roots(i + 1) * std::complex<double>(1, 1);
  }

  // Iterative solver.
  const int kMaxNumIterations = 100;
  const double kMaxRootChange = 1e-10;
  for (int iter = 0; iter < kMaxNumIterations; ++iter) {
    double max_root_change = 0.0;
    for (int i = 0; i < degree; ++i) {
      const std::complex<double> root_i = roots(i);
      std::complex<double> numerator = coeffs[0];
      std::complex<double> denominator = coeffs[0];
      for (int j = 0; j < degree; ++j) {
        numerator = numerator * root_i + coeffs[j + 1];
        if (i != j) {
          denominator = denominator * (root_i - roots(j));
        }
      }
      const std::complex<double> root_i_change = numerator / denominator;
      roots(i) = root_i - root_i_change;
      max_root_change =
          std::max(max_root_change, std::abs(root_i_change.real()));
      max_root_change =
          std::max(max_root_change, std::abs(root_i_change.imag()));
    }

    // Break, if roots do not change anymore.
    if (max_root_change < kMaxRootChange) {
      break;
    }
  }

  if (real != nullptr) {
    real->resize(degree);
    *real = roots.real();
  }
  if (imag != nullptr) {
    imag->resize(degree);
    *imag = roots.imag();
  }

  return true;
}

bool FindPolynomialRootsCompanionMatrix(const Eigen::VectorXd& coeffs_all,
                                        Eigen::VectorXd* real,
                                        Eigen::VectorXd* imag) {
  CHECK_GE(coeffs_all.size(), 2);

  Eigen::VectorXd coeffs = RemoveLeadingZeros(coeffs_all);

  const int degree = coeffs.size() - 1;

  if (degree <= 0) {
    return false;
  } else if (degree == 1) {
    return FindLinearPolynomialRoots(coeffs, real, imag);
  } else if (degree == 2) {
    return FindQuadraticPolynomialRoots(coeffs, real, imag);
  }

  // Remove the coefficients where zero is a solution.
  coeffs = RemoveTrailingZeros(coeffs);

  // Check if only zero is a solution.
  if (coeffs.size() == 1) {
    if (real != nullptr) {
      real->resize(1);
      (*real)(0) = 0;
    }
    if (imag != nullptr) {
      imag->resize(1);
      (*imag)(0) = 0;
    }
    return true;
  }

  // Fill the companion matrix.
  Eigen::MatrixXd C(coeffs.size() - 1, coeffs.size() - 1);
  C.setZero();
  for (Eigen::MatrixXd::Index i = 1; i < C.rows(); ++i) {
    C(i, i - 1) = 1;
  }
  C.row(0) = -coeffs.tail(coeffs.size() - 1) / coeffs(0);

  // Solve for the roots of the polynomial.
  Eigen::EigenSolver<Eigen::MatrixXd> solver(C, false);
  if (solver.info() != Eigen::Success) {
    return false;
  }

  // If there are trailing zeros, we must add zero as a solution.
  const int effective_degree =
      coeffs.size() - 1 < degree ? coeffs.size() : coeffs.size() - 1;

  if (real != nullptr) {
    real->resize(effective_degree);
    real->head(coeffs.size() - 1) = solver.eigenvalues().real();
    if (effective_degree > coeffs.size() - 1) {
      (*real)(real->size() - 1) = 0;
    }
  }
  if (imag != nullptr) {
    imag->resize(effective_degree);
    imag->head(coeffs.size() - 1) = solver.eigenvalues().imag();
    if (effective_degree > coeffs.size() - 1) {
      (*imag)(imag->size() - 1) = 0;
    }
  }

  return true;
}

}  // namespace colmap


#include "./../../header/pearce.h"
#include "./../../header/analyzer.h"
#include "./../../header/visualize.h"

//STL
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iterator>
//Boost
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/copy.hpp>
#include <boost/property_map/property_map.hpp>

using namespace boost;

void Pearce::print_graph(){
    GraphComponent::print_graph_file(p);
}

void Pearce::print_result_max(UtilityStructs::StorageItems &s){
    std::vector<int> rindex = s.auxilary;
    Visualize vis;
    int sizeOfGraph = num_vertices(p);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    v_p id = get(&VertexProperty::index ,p);

    int max = *max_element(std::begin(rindex), std::end(rindex));
    for(int i = 0; i <= max; i++){
         int count_component = 0;
          vis.printLine("");
            std::stringstream buffer ;
            buffer << std::setw(40) << std::left << "The Strongly Connected Component is" ;
            buffer << ": ";
          for(vp=vertices(p); vp.first != vp.second; vp.first++){
                 Vertex_t v = *vp.first;
                if(rindex[id[v]] == i){
                    count_component++;
                    buffer << id[v]+1 <<  " " ;
                }
         }
        vis.printLine(buffer.str());
            std::stringstream buffer2;
           // vis.printLine("");
            buffer2 << std::setw(40) << std::left << "Number of elements in the component is" ;
            buffer2 << ": "<< count_component ;
            vis.printLine(buffer2.str());

    }
    vis.printProgramBottom();
    std::stringstream buffer3;
            buffer3 << std::setw(40) << std::left <<"Completion Time" ;
            buffer3 << ": " << s.duration << " miliseconds" ;
            std::stringstream buffer4;
            buffer4 << std::setw(40)<< std::left << "Total Execution Size";
            buffer4 << ": " << s.total_bytes / 1000.f << " KBs";
            vis.printLine(buffer3.str());
            vis.printLine(buffer4.str());

}

void Pearce::print_result_min(UtilityStructs::StorageItems &s){
    std::vector<int> rindex = s.auxilary;
    Visualize vis;
    int sizeOfGraph = num_vertices(p);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    v_p id = get(&VertexProperty::index ,p);
     int min = *min_element(std::begin(rindex), std::end(rindex));
    for(int i = sizeOfGraph-1; i >= min; i--){
         int count_component = 0;
          vis.printLine("");
            std::stringstream buffer ;
            buffer << std::setw(40) << std::left << "The Strongly Connected Component is" ;
            buffer << ": ";
          for(vp=vertices(p); vp.first != vp.second; vp.first++){
                 Vertex_t v = *vp.first;
                if(rindex[id[v]] == i){
                    buffer << id[v]+1 <<  " " ;
                    count_component++;
                }
         }
        vis.printLine(buffer.str());
            std::stringstream buffer2;
           // vis.printLine("");
            buffer2 << std::setw(40) << std::left << "Number of elements in the component is" ;
            buffer2 << ": "<< count_component ;
            vis.printLine(buffer2.str());

    }
    vis.printProgramBottom();
    std::stringstream buffer3;
            buffer3 << std::setw(40) << std::left <<"Completion Time" ;
            buffer3 << ": " << s.duration << " miliseconds" ;
            std::stringstream buffer4;
            buffer4 << std::setw(40)<< std::left << "Total Execution Size";
            buffer4 << ": " << s.total_bytes / 1000.f << " KBs";
            vis.printLine(buffer3.str());
            vis.printLine(buffer4.str());

}
void Pearce::DFS() {
    int index = 0;
    v_p id = get(&VertexProperty::index ,p);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    int sizeOfGraph = num_vertices(p);
    std::vector<int> visited(sizeOfGraph,false);
    for(vp = vertices(p); vp.first != vp.second; vp.first++){
        Vertex_t v = *vp.first;
        if(!visited[id[v]]){
            visit(v,visited,index);
        }
    }
    std::cout << "\nPearce DFS implementation exited succesfully" << std::endl;
    std::cout << "Discovery Order is: " << std::endl;
    for(vp=vertices(p); vp.first != vp.second; vp.first++){
        Vertex_t v = *vp.first;
        std::cout << "Node(" << id[v] << ") --> " << visited[id[v]] << std::endl;
    }
}

void Pearce::visit(Vertex_t &v,std::vector<int> &visited,int &index) {
    v_p id = get(&VertexProperty::index, p);
    visited[id[v]] = index++;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
    for (boost::tie(out_i, out_end) = out_edges(v, p); out_i != out_end; ++out_i){
        Edge e = *out_i;
        Vertex_t w = target(e, p);
        if (visited[id[w]] == false){
            visit(w, visited,index);
        }
    }

}

UtilityStructs::StorageItems Pearce::Pea_Find_SCC1() {
    float ms_duration =0;
    int index = 0;
    int c= 0;
    int stackCount = 0;
    int sizeOfGraph = num_vertices(p);
    std::vector<bool> visited(sizeOfGraph,false);
    std::vector<int> rindex (sizeOfGraph,0);
    std::vector<bool> inComponent(sizeOfGraph,false);
    std::vector<Vertex_t> Stack;
    v_p id = get(&VertexProperty::index, p);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
//    std::cout << "\nPearce SCC implementation mark 1 exited succesfully" << std::endl;
    //Create the Timer object
    {
        UtilityStructs::Timer timer;
    for(vp=vertices(p); vp.first != vp.second; vp.first++){
        Vertex_t v = *vp.first;
        if(!visited[id[v]]){
            visit_scc1(v,visited,rindex,inComponent,Stack,index,c,stackCount);
        }
    }
    ms_duration = timer.stop();
    }
    //Storage info collection
    //print_result_max(rindex);
    size_t total_bytes = 0;
    total_bytes += sizeof(visited[0]) * visited.size();
    total_bytes += sizeof(rindex[0]) * rindex.size();
    total_bytes += sizeof(rindex[0]) * inComponent.size();
    total_bytes += sizeof(Stack[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(p);
    s.edgeCount = num_edges(p);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    s.auxilary = rindex;
    return s;
}

void Pearce::visit_scc1(Vertex_t &v,
                        std::vector<bool> &visited,
                        std::vector<int> &rindex,
                        std::vector<bool> &inComponent,
                        std::vector<Vertex_t> &Stack,
                        int &index,
                        int &c,
                        int &stackCount){

    v_p id = get(&VertexProperty::index, p);
    bool root= true;
    visited[id[v]] = true;
    rindex[id[v]] = index++;
    inComponent[id[v]] = false;

    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;

    for (boost::tie(out_i, out_end) = out_edges(v, p); out_i != out_end; ++out_i){
        Edge e = *out_i;
        Vertex_t w = target(e, p);
        if (visited[id[w]] == false){
            visit_scc1(w, visited,rindex,inComponent,Stack, index,c,stackCount);
        }
        if(! inComponent[id[w]] && (rindex[id[w]] < rindex[id[v]])){
            rindex[id[v]] = rindex[id[w]];
            root = false;
        }
    }
    if(root){
        inComponent[id[v]] = true ;
        if(!Stack.empty()){
            while(!Stack.empty() && (rindex[id[v]] <= rindex[id[Stack.back()]])){
                Vertex_t w = Stack.back();
                Stack.pop_back();
                rindex[id[w]] = c;
                inComponent[id[w]] = true;
            }

        }
        rindex[id[v]] = c;
        c = c+1;
    }else{
        Stack.push_back(v);
        stackCount++;
    }
}

UtilityStructs::StorageItems Pearce::Pea_Find_SCC2(){
    float ms_duration;
    int index = 1;
    int sizeOfGraph = num_vertices(p);
    int c = sizeOfGraph -1;
    int stackCount = 0;
    std::vector<int> rindex(sizeOfGraph, 0);
    std::vector<Vertex_t> Stack;
    v_p id = get(&VertexProperty::index, p);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    //Create the Timer object
    //TODO
//    std::cout << "\nPearce SCC implementation mark 2 exited succesfully" << std::endl;
    {
        UtilityStructs::Timer timer;
        for (vp = vertices(p); vp.first != vp.second; vp.first++){
        Vertex_t v = *vp.first;
        if (rindex[id[v]] == 0){
            visit_scc2(v,rindex, Stack, index, c,stackCount);
        }
        }
    ms_duration = timer.stop();
    }
    //Storage info collection
    //print_result_min(rindex);
    size_t total_bytes = 0;
    total_bytes += sizeof(c);
    total_bytes += sizeof(rindex[0]) * rindex.size();
    total_bytes += sizeof(Stack[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(p);
    s.edgeCount = num_edges(p);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    s.auxilary = rindex;
    return s;
}

void Pearce::visit_scc2(Vertex_t &v,
                        std::vector<int> &rindex,
                        std::vector<Vertex_t> &Stack,
                        int &index,
                        int &c,
                        int &stackCount) {

    v_p id = get(&VertexProperty::index, p);
    bool  root = true;
    rindex[id[v]] = index++;

    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;

    for (boost::tie(out_i, out_end) = out_edges(v, p); out_i != out_end; ++out_i){
        Edge e = *out_i;
        Vertex_t w = target(e, p);
        if (rindex[id[w]] == 0){
            visit_scc2(w, rindex, Stack, index, c,stackCount);
        }
        if(rindex[id[w]] < rindex[id[v]]){
            rindex[id[v]] = rindex[id[w]] ;
            root = false;
        }
    }

    if(root){
        index--;
        if(!Stack.empty()){
            while(! Stack.empty() && (rindex[id[v]] <= rindex[id[Stack.back()]])){
                Vertex_t w = Stack.back();
                Stack.pop_back();
                rindex[id[w]] = c;
                index--;
            }
        }
        rindex[id[v]] = c;
        c=c-1;
    }else{
        Stack.push_back(v);
        stackCount++;
    }
}

UtilityStructs::StorageItems Pearce::Pea_Find_SCC3() {
    float ms_duration;
    int index = 1;
    int sizeOfGraph = num_vertices(p);
    int c = sizeOfGraph -1;
    int stackCount_i = 0;
    int stackCount_v = 0;
    std::vector<bool> root(sizeOfGraph,false);
    std::vector<int> rindex(sizeOfGraph,0);
    std::vector<Vertex_t> vStack;
    std::vector<int> iStack;
    v_p id = get(&VertexProperty::index,p);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    //Create the Timer Object
    {
        UtilityStructs::Timer timer;
        for (vp = vertices(p); vp.first != vp.second; vp.first++){
        Vertex_t v = *vp.first;
        if (rindex[id[v]] == 0){
            visit_scc3(v, root,rindex,vStack,iStack,index,c,stackCount_i,stackCount_v);
        }
    }
        ms_duration = timer.stop();
    }
    //print_result_min(rindex);
    //Storage info collection
    size_t total_bytes= 0;
    total_bytes += sizeof(root[0]) * root.size();
    total_bytes += sizeof(rindex[0]) * rindex.size();
    total_bytes += sizeof(iStack[0] )* stackCount_i;
    total_bytes += sizeof(vStack[0]) * stackCount_v;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(p);
    s.edgeCount = num_edges(p);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    s.auxilary = rindex;
    return s;

}

void Pearce::visit_scc3(Vertex_t &v,
                        std::vector<bool> &root,
                        std::vector<int> &rindex,
                        std::vector<Vertex_t> &vStack,
                        std::vector<int> &iStack,
                        int &index,
                        int &c,
                        int &stackCount_i,
                        int &stackCount_v) {
    beginVisiting(v,root,rindex,vStack,iStack,index,stackCount_i,stackCount_v);
    while(!vStack.empty()){
        visitLoop(root,rindex,vStack,iStack,index,c,stackCount_i,stackCount_v);
    }
}

void Pearce::visitLoop( std::vector<bool> &root,
                        std::vector<int> &rindex,
                        std::vector<Vertex_t> &vStack,
                        std::vector<int> &iStack,
                        int &index,
                        int &c,
                        int &stackCount_i,
                        int &stackCount_v) {
    Vertex_t v = vStack[0];
    int i = iStack[0];

    int out_edges = out_degree(v,p);

    while(i <= out_edges){
        if (i > 0){
            int k = i-1;
            finishEdge(v,k,rindex,root);
        }
        if ( i < out_edges && beginEdge(v,i,root,rindex,vStack,iStack,index,c,stackCount_i,stackCount_v)){
            return;
        }
        i = i+1;
    }
    finishVisiting(v,root,rindex,vStack,iStack,index,c,stackCount_i,stackCount_v);
}

void Pearce::beginVisiting(Vertex_t &v,
                           std::vector<bool> &root,
                           std::vector<int> &rindex,
                           std::vector<Vertex_t> &vStack,
                           std::vector<int> &iStack,
                           int &index,
                           int &stackCount_i,
                           int &stackCount_v){

    vStack.insert(vStack.begin(),v);
    iStack.insert(iStack.begin(),0);
    stackCount_i ++;
    stackCount_v ++;
    v_p id = get(&VertexProperty::index, p);
    root[id[v]] = true;
    rindex[id[v]] = index;
    index = index+1;
}

void Pearce::finishVisiting(Vertex_t &v,
                            std::vector<bool> &root,
                            std::vector<int> &rindex,
                            std::vector<Vertex_t> &vStack,
                            std::vector<int> &iStack,
                            int &index,
                            int &c,
                            int &stackCount_i,
                            int &stackCount_v) {
    v_p id = get(&VertexProperty::index, p);
    vStack.erase(vStack.begin());
    iStack.erase(iStack.begin());
    if(root[id[v]]){
        index--;
        while(!vStack.empty() && (rindex[id[v]] <= rindex[id[vStack.back()]])){
            Vertex_t w = vStack.back();
            vStack.pop_back();
            rindex[id[w]] = c;
            index--;
        }
        rindex[id[v]] = c;
        c = c-1;
    }else{
        vStack.push_back(v);
        stackCount_v++;
    }

}

bool Pearce::beginEdge(Vertex_t &v,
                        int &k,
                        std::vector<bool> &root,
                        std::vector<int> &rindex,
                        std::vector<Vertex_t> &vStack,
                        std::vector<int> &iStack,
                        int &index,
                        int &c,
                        int &stackCount_i,
                        int &stackCount_v) {

    v_p id = get(&VertexProperty::index, p);
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
    boost::tie(out_i,out_end) = out_edges(v,p);
    out_i += k;
    Edge e = *out_i;
    Vertex_t w = target(e, p);
    if(rindex[id[w]] == 0){
        iStack.erase(iStack.begin());
        iStack.insert(iStack.begin(),k+1);
        beginVisiting(w,root,rindex,vStack,iStack,index,stackCount_i,stackCount_v);
        return true;
    }else{
        return false;
    }

}

void Pearce::finishEdge(Vertex_t &v,
                        int &k,
                        std::vector<int> &rindex,
                        std::vector<bool> &root) {
    v_p id = get(&VertexProperty::index, p);
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator out_i, out_end;
    boost::tie(out_i,out_end) = out_edges(v,p);
    out_i += k;
    Edge e = *out_i;
    Vertex_t w = target(e, p);
    if(rindex[id[w]] < rindex[id[v]]){
        rindex[id[v]] = rindex[id[w]];
        root[id[v]] = false;
    }

}


// this must define the same symbol as the main module file (numpy requirement)
#define PY_ARRAY_UNIQUE_SYMBOL andres_graph_PyArray_API
#define NO_IMPORT_ARRAY

#include <Python.h>
#include <boost/python.hpp>
#include <vigra/numpy_array.hxx>
#include <vigra/numpy_array_converters.hxx>

#include "andres/graph/grid-graph.hxx"
#include "andres/graph/graph.hxx"
#include "andres/graph/components.hxx"

namespace bp = boost::python;
namespace agraph = andres::graph;

// struct FusionMoveMcSettings {
//     std::size_t maxNumberOfIterations { 1000 };
//     std::size_t maxNumberOfIterationsWithoutImprovement { 200 };
//     double nodeLimit = {0.1};
//     double sigma{ 1.0};
//     int verbose { 1 };
// };

template<class OG, class LG>
vigra::NumpyAnyArray pyImageToLocalWeights(
    const OG & og,
    const LG & lg,
    vigra::NumpyArray<2, float>   image,
    vigra::NumpyArray<1, float> out
){

    vigra::TinyVector<int, 1> shape(lg.numberOfEdges());
    out.reshapeIfEmpty(shape);
    for(auto e=0; e<og.numberOfEdges(); ++e){
        auto u = og.vertexOfEdge(e, 0);
        auto v = og.vertexOfEdge(e, 1);
        out[e] = (image[u] + image[v])/2.0;
    }
    return out;
}

template<class OG, class LG>
vigra::NumpyAnyArray pyLiftedEdgeLabesToImage(
    const OG & og,
    const LG & lg,
    vigra::NumpyArray<1, uint8_t> edgeLabels,
    vigra::NumpyArray<2, uint64_t>   image
){

    struct SubgraphWithCut { // a subgraph with cut mask
        SubgraphWithCut(const vigra::MultiArrayView<1,uint8_t> & labels,
                        std::vector<std::size_t> const& edge_in_lifted_graph)
            : labels_(labels), edge_in_lifted_graph_(edge_in_lifted_graph)
            {}
        bool vertex(const std::size_t v) const
            { return true; }
        bool edge(const std::size_t e) const
            { return labels_[edge_in_lifted_graph_[e]] == 0; }

        std::vector<std::size_t> const& edge_in_lifted_graph_;
        const vigra::MultiArrayView<1,uint8_t> & labels_;
    };

    vigra::TinyVector<int, 2> shape(og.shape(0),og.shape(1));
    image.reshapeIfEmpty(shape);

    std::vector<size_t> edgeInLiftedGraph(og.numberOfEdges());
    for (std::size_t i = 0; i < og.numberOfEdges(); ++i){
        auto v0 = og.vertexOfEdge(i, 0);
        auto v1 = og.vertexOfEdge(i, 1);
        edgeInLiftedGraph[i] = lg.findEdge(v0, v1).second;
    }
    auto edgeLabels_ = vigra::MultiArrayView<1,uint8_t>(edgeLabels);
    agraph::ComponentsBySearch<OG > components;
    components.build(og, SubgraphWithCut(edgeLabels_, edgeInLiftedGraph));

    for(size_t i=0; i<image.size(); ++i){
        image[i] =components.labels_[i];
    }
    return image;
}

void exportLiftedMc(){
    typedef agraph::GridGraph<2> GridGraph2D;
    typedef agraph::GridGraph<3> GridGraph3D;
    typedef agraph::Graph<> Graph;

    bp::def("imageToLocalWeights",vigra::registerConverters(&pyImageToLocalWeights<GridGraph2D,Graph>),
        (
            bp::arg("graph"),
            bp::arg("liftedGraph"),
            bp::arg("image"),
            bp::arg("out") = bp::object()
        )
    );

    bp::def("liftedEdgeLabesToImage",vigra::registerConverters(&pyLiftedEdgeLabesToImage<GridGraph2D,Graph>),
        (
            bp::arg("graph"),
            bp::arg("liftedGraph"),
            bp::arg("edgeLabels"),
            bp::arg("out") = bp::object()
        )
    );
}


// Copyright 2015 Conrad Sanderson (http://conradsanderson.id.au)
// Copyright 2015 National ICT Australia (NICTA)
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// ------------------------------------------------------------------------

#include <armadillo>
#include "catch.hpp"

using namespace arma;

TEST_CASE("fn_cross_1")
  {
  vec a = { 0.1,  2.3,  4.5 };
  vec b = { 6.7,  8.9, 10.0 };

  vec c = {-17.050, 29.150, -14.520 };

  REQUIRE( accu(abs(cross(a,b) - c)) == Approx(0.0) );

  vec x;

  REQUIRE_THROWS( x = cross(randu<vec>(4), randu<vec>(4)) );
  }


#include <EigenFaceMaskedArray.h>
#include <EigenFace.h>
#include <EigenFaceData.h>

#include <QColor>
#include <QImage>
#include <QString>
#include <QVector>

#include <GreyImage.h>
#include <Return.h>

#include <math.h>

QImage EigenFaceMaskedArray::toImage(QImage::Format Format, bool fullMask) const
{
    if ( ! isValid())
        return QImage();

    QSize sz(efData->mask().size());
    QImage image(sz, QImage::Format_Indexed8);
    image.setNumColors(256);
    image.setColorTable(GreyImage::greyColorTable());
    image.fill(128);

    int x = 0;
    for (int row = 0; row < sz.width() ; ++row)
        for (int col = 0; col < sz.height(); ++col)
            if (efData->mask().at(col, row))
            {
        if (fullMask || efData->outputMask().at(col, row))
        {
            qreal f = data.at(x);
            if (f < 0.0)
                image.setPixel(col, row, (int)128 - (128.0 * f / minData));
            else
                image.setPixel(col, row, (int)127 + (128.0 * f / maxData));
        }
        ++x;
    }
    image = image.convertToFormat(Format);

    return image;
} // toImage()

Return EigenFaceMaskedArray::fromImage(const QImage & image)
{
    if ( ! efData || ! efData->isValid())
        return Return(EigenFace::ReturnInvalidStructure, "FaceData");
    if ( ! efData->mask().isValid())
        return Return(EigenFace::ReturnInvalidStructure, "BitMask");
    if ( ! efData->weights().isValid())
        return Return(EigenFace::ReturnInvalidStructure, "WeightMask");

    int rows = efData->eigenSize().height();
    int cols = efData->eigenSize().width();

    clear();
    data.reserve(efData->activePixels());
    qreal totalGrey = 0.0;

    for (int y = 0; y < rows; ++y)
        for (int x = 0; x < cols; ++x)
            if (efData->mask().at(x,y))
            {
        qreal grey;
        if (QImage::Format_Indexed8 == image.format())
        {
            int pixel = image.pixel(x, y);
            grey = (qreal)pixel / 255.0;
        }
        else
        {
            QRgb pixel = image.pixel(x, y);
            // accumulateColor(pixel);
            grey = (qreal)qGray(pixel) / 255.0;
        }
        grey *= efData->weights().at(data.size());
        totalGrey += grey;
        if (grey < minData)	minData = grey;
        if (grey > maxData)	maxData = grey;
        data.append(grey);
    } // if, for, for

    if ( ! isValid())
        return Return(EigenFace::ReturnInvalidStructure, "Invalid Count");

    qreal averageGrey = totalGrey / (qreal)data.size();
    qreal sumSquares = subtract(averageGrey);

    qreal variance = sqrt(sumSquares);
    divideBy(variance);
    // IEIGENFACE_PRELIMINARY
    /*
	QImage varImage = toImage(QImage::Format_Indexed8, false);
	varImage.save("varImage.jpg");
*/
    return Return();
} // fromImage()

Return EigenFaceMaskedArray::generateEigenImage(const QImage & image, Eyes eyes)
{
    if (image.isNull())
        return Return(EigenFace::ReturnNullImage, "InputImage");
    if ( ! efData)
        return Return(EigenFace::ReturnNoData);
    Eyes eigenEyes = efData->eigenEyes();
    if (eigenEyes.isNull())
        return Return(EigenFace::ReturnInvalidStructure, "eigenEyes");
    QSize eigenSize = efData->eigenSize();
    if ( ! eigenSize.isValid())
        return Return(EigenFace::ReturnInvalidStructure, "eigenSize");
    if (eyes.isNull())
        return Return(EigenFace::ReturnNoEyes);
    eigenImage = EigenFace::normalize(image, eyes, eigenSize, eigenEyes);
    if (eigenImage.isNull())
        return Return(EigenFace::ReturnNullImage, "EigenFace");
    return Return();
} // generateEigenImage()

Return EigenFaceMaskedArray::generateEigenImage(const GreyImage & gi, Eyes eyes)
{
    if (gi.isNull())
        return Return(EigenFace::ReturnNullImage, "InputImage");
    if ( ! efData)
        return Return(EigenFace::ReturnNoData);
    Eyes eigenEyes = efData->eigenEyes();
    if (eigenEyes.isNull())
        return Return(EigenFace::ReturnInvalidStructure, "eigenEyes");
    QSize eigenSize = efData->eigenSize();
    if ( ! eigenSize.isValid())
        return Return(EigenFace::ReturnInvalidStructure, "eigenSize");
    if (eyes.isNull())
        return Return(EigenFace::ReturnNoEyes);
    eigenImage = EigenFace::normalize(gi, eyes, eigenSize, eigenEyes);
    if (eigenImage.isNull())
        return Return(EigenFace::ReturnNullImage, "EigenFace");
    return Return();
} // generateEigenImage()

Return EigenFaceMaskedArray::generate(const QImage & image,
                                           const Eyes eyes,
                                           const EigenFaceVectorKey vecKey)
{
    Return rtn;

    if ( ! efData)
        return Return(EigenFace::ReturnNoData);
    if ( ! efData->meanFace(vecKey.meanId()).isValid())
        return Return(EigenFace::ReturnInvalidStructure, "MeanFace");

    rtn = generateEigenImage(image, eyes);
    if (rtn.isError())
        return rtn;

    rtn = fromImage(eigenImage);
    if (rtn.isError())
        return rtn;

    Residual = subtract(efData->meanFace(vecKey.meanId()));
    setVectorKey(vecKey);
    return rtn;
} // generate()

Return EigenFaceMaskedArray::generate(const GreyImage & gi,
                                           const Eyes eyes,
                                           const EigenFaceVectorKey vecKey)
{
    Return rtn;

    if ( ! efData)
        return Return(EigenFace::ReturnNoData);
    if ( ! efData->meanFace(vecKey.meanId()).isValid())
        return Return(EigenFace::ReturnInvalidStructure, "MeanFace");

    rtn = generateEigenImage(gi, eyes);
    if (rtn.isError())
        return rtn;

    rtn = fromImage(eigenImage);
    if (rtn.isError())
        return rtn;

    Residual = subtract(efData->meanFace(vecKey.meanId()));
    setVectorKey(vecKey);
    return rtn;
} // generate()


#include <cdrm_legged/leg_config_generator.h>

#include <cdrm_legged/leg_model.h>

#include <angles/angles.h>
#include <boost/graph/adjacency_list.hpp>
#include <eigen_stl_containers/eigen_stl_vector_container.h>
#include <cdrm/cdrm.h>
#include <cdrm/voxelise.h>
#include <geometric_shapes/shapes.h>
#include <moveit/planning_scene/planning_scene.h>
#include <ros/console.h>
#include <swri_profiler/profiler.h>

#include <queue>

namespace cdrm_legged
{
LegConfigGenerator::LegConfigGenerator(const planning_scene::PlanningSceneConstPtr &planning_scene)
  : planning_scene_(planning_scene)
{
  heuristic_ = [](const LegModel &, unsigned int, double angle_a, unsigned int, double angle_b) {
    return std::abs(angle_a) < std::abs(angle_b);
  };
}

LegConfigs LegConfigGenerator::generateLegConfigs(const Eigen::Isometry3d &body_tf, const LegModel &leg) const
{
  SWRI_PROFILE("generate_leg_configs");

  const auto *cdrm = leg.cdrm_;

  LegConfigs configs;

  Eigen::Isometry3d leg_tf = body_tf * leg.tf_;
  Eigen::Isometry3d leg_tf_inv = leg_tf.inverse();

  std::vector<cdrm::VertexDescriptor> contacts;
  EigenSTL::vector_Vector3d contact_normals;
  std::set<cdrm::VertexDescriptor> colliding_vertices;
  std::set<cdrm::EdgeDescriptor> colliding_edges;

  const auto callback = [&](const Eigen::Vector3d &p, const Eigen::Vector3d &n)
  {
    const auto key = cdrm->pointToKey(leg_tf_inv * p);

    const auto cell_contacts = cdrm->contacts_.equal_range(key);
    const auto cell_vertices = cdrm->colliding_vertices_.equal_range(key);
    const auto cell_edges = cdrm->colliding_edges_.equal_range(key);

    for (auto it = cell_contacts.first; it != cell_contacts.second; ++it)
    {
      contacts.push_back(it->second);
      contact_normals.push_back(n);
    }

    for (auto it = cell_vertices.first; it != cell_vertices.second; ++it)
      colliding_vertices.insert(it->second);

    for (auto it = cell_edges.first; it != cell_edges.second; ++it)
      colliding_edges.insert(it->second);
  };

  Eigen::AlignedBox3d aabb;
  aabb.extend(leg_tf * leg.cdrm_->aabb_.min());
  aabb.extend(leg_tf * leg.cdrm_->aabb_.max());
  Eigen::Vector3d min = aabb.min();
  Eigen::Vector3d max = aabb.max();

  for (const auto &object : *(planning_scene_->getWorld()))
  {
    const auto &shapes = object.second->shapes_;
    const auto &poses = object.second->shape_poses_;

    for (std::size_t i = 0; i < shapes.size(); ++i)
    {
      if (shapes[i]->type != shapes::MESH)
        continue;

      const auto *mesh = static_cast<const shapes::Mesh *>(shapes[i].get());
      cdrm::voxelise(*mesh, leg.cdrm_->resolution_, callback, poses[i], &min, &max);
    }
  }

  for (std::size_t i = 0; i < contacts.size(); ++i)
  {
    const auto contact = contacts[i];

    if (colliding_vertices.count(contact))
      continue;

    // Get the angle between the foot and the surface normal.
    const Eigen::VectorXd &q = leg.cdrm_->getVertexConfig(contact);
    const Eigen::Vector3d direction = (body_tf * leg.tf_ * leg.getTipTransform(q)).linear().col(2);
    const Eigen::Vector3d &normal = contact_normals[i];
    const double angle = std::acos(direction.dot(-normal));

    if (angle > angles::from_degrees(45))
      continue;

    configs.contacts_.push_back(contact);
    configs.normals_.push_back(normal);
  }

  // Sort the contacts by heuristic.
  // std::sort(configs.contacts_.begin(), configs.contacts_.end(), [&, this](unsigned int a, unsigned int b) {
  //   return heuristic_(leg, a, contact_angles.at(a), b, contact_angles.at(b));
  // });

  // Generate disjoint sets, starting at contacts (as that is what we want to connect to).
  unsigned int num_vertices = boost::num_vertices(leg.cdrm_->roadmap_);

  DisjointSets disjoint(num_vertices);

  std::queue<int> queue;
  std::set<int> seen;

  for (const auto &contact : configs.contacts_)
  {
    queue.push(contact);
    seen.insert(contact);
  }

  while (!queue.empty())
  {
    auto source = queue.front();
    auto out = boost::out_edges(source, cdrm->roadmap_);
    queue.pop();

    for (auto it = out.first; it != out.second; ++it)
    {
      auto target = boost::target(*it, cdrm->roadmap_);

      if (seen.count(target))
        continue;

      if (colliding_edges.count(*it))
        continue;

      seen.insert(target);

      disjoint.merge(source, target);
    }
  }

  configs.reachable_.resize(configs.contacts_.size());

  // Copy it across to the reachable configurations.
  for (unsigned int i = 0; i < num_vertices; ++i)
  {
    for (std::size_t j = 0; j < configs.contacts_.size(); ++j)
    {
      if (disjoint.connected(i, configs.contacts_[j]))
        configs.reachable_[j].insert(i);
    }
  }

  return configs;
}
}


#pragma once

#include <Eigen/Dense>

#include "GNAObject.hh"
#include "Neutrino.hh"
#include "config_vars.h"

namespace GNA {
  namespace GNAObjectTemplates {
    template<typename FloatType>
    class OscillationVariablesT;

    template<typename FloatType>
    class PMNSVariablesT;
  }
}
using OscillationVariables = GNA::GNAObjectTemplates::OscillationVariablesT<double>;
using PMNSVariables = GNA::GNAObjectTemplates::PMNSVariablesT<double>;

class OscProbPMNSBase: public GNAObject,
                       public TransformationBind<OscProbPMNSBase> {
protected:
  OscProbPMNSBase(Neutrino from, Neutrino to);

  template <int I, int J>
  double DeltaMSq() const;

  template <int I, int J>
  double weight() const;

  double weightCP() const;

  std::unique_ptr<OscillationVariables> m_param;
  std::unique_ptr<PMNSVariables> m_pmns;

  int m_alpha, m_beta, m_lepton_charge;
};

namespace GNA {
  namespace GNAObjectTemplates {
    template<typename FloatType>
    class OscProbPMNST: public OscProbPMNSBase,
                        public TransformationBind<OscProbPMNST<FloatType>, FloatType, FloatType> {
    protected:
      using BaseClass = GNAObjectT<FloatType,FloatType>;
    public:
      using TransformationBind<OscProbPMNST<FloatType>, FloatType, FloatType>::transformation_;
      using typename BaseClass::FunctionArgs;

      OscProbPMNST<FloatType>(Neutrino from, Neutrino to, std::string l_name="L");

      template <int I, int J>
      void calcComponent(FunctionArgs& fargs);
      void calcComponentCP(FunctionArgs& fargs);
      void calcSum(FunctionArgs& fargs);
      void calcFullProb(FunctionArgs& fargs);
    #ifdef GNA_CUDA_SUPPORT
      void calcFullProbGpu(FunctionArgs& fargs);
      template <int I, int J>
      void gpuCalcComponent(FunctionArgs& fargs);
      void gpuCalcComponentCP(FunctionArgs& fargs);
      void gpuCalcSum(FunctionArgs& fargs);
    #endif

    protected:
      variable<FloatType> m_L;
    };
  }
}

using OscProbPMNS = GNA::GNAObjectTemplates::OscProbPMNST<double>;

class OscProbAveraged: public OscProbPMNSBase,
                       public TransformationBind<OscProbAveraged> {
public:
  using TransformationBind<OscProbAveraged>::transformation_;

  OscProbAveraged(Neutrino from, Neutrino to);
private:
  void CalcAverage(FunctionArgs fargs);
};

class OscProbPMNSMult: public OscProbPMNSBase,
                       public TransformationBind<OscProbPMNSMult> {
public:
  using TransformationBind<OscProbPMNSMult>::transformation_;

  OscProbPMNSMult(Neutrino from, Neutrino to, std::string l_name="Lavg");

  template <int I, int J>
  void calcComponent(FunctionArgs fargs);
  void calcSum(FunctionArgs fargs);
protected:
  variable<double> m_Lavg;

  variable<double> m_weights;
};


// Copyright (c) 2016-2020 The ZCash developers
// Copyright (c) 2020 The PIVX developers
// Copyright (c) 2021 The REBEL Coin developers
// Distributed under the MIT software license, see the accompanying
// file COPYING or http://www.opensource.org/licenses/mit-license.php.

#include "test/test_rebelcoin.h"
#include "test/librust/utiltest.h"

#include "sapling/address.h"
#include "sapling/sapling_util.h"

#include <boost/test/unit_test.hpp>
#include <univalue.h>

#include "data/sapling_key_components.json.h"

// In script_tests.cpp
extern UniValue read_json(const std::string& jsondata);

BOOST_FIXTURE_TEST_SUITE(sapling_keystore_tests, BasicTestingSetup)

BOOST_AUTO_TEST_CASE(saplingKeys) {
    // ["sk, ask, nsk, ovk, ak, nk, ivk, default_d, default_pk_d, note_v, note_r, note_cm, note_pos, note_nf"],
    UniValue sapling_keys = read_json(std::string(json_tests::sapling_key_components, json_tests::sapling_key_components + sizeof(json_tests::sapling_key_components)));

    // Skipping over comments in sapling_key_components.json file
    for (size_t i = 2; i < 12; i++) {
        uint256 skSeed, ask, nsk, ovk, ak, nk, ivk;
        skSeed.SetHex(sapling_keys[i][0].getValStr());
        ask.SetHex(sapling_keys[i][1].getValStr());
        nsk.SetHex(sapling_keys[i][2].getValStr());
        ovk.SetHex(sapling_keys[i][3].getValStr());
        ak.SetHex(sapling_keys[i][4].getValStr());
        nk.SetHex(sapling_keys[i][5].getValStr());
        ivk.SetHex(sapling_keys[i][6].getValStr());

        diversifier_t default_d;
        std::copy_n(ParseHex(sapling_keys[i][7].getValStr()).begin(), 11, default_d.begin());

        uint256 default_pk_d;
        default_pk_d.SetHex(sapling_keys[i][8].getValStr());

        auto sk = libzcash::SaplingSpendingKey(skSeed);

        // Check that expanded spending key from primitives and from sk are the same
        auto exp_sk_2 = libzcash::SaplingExpandedSpendingKey(ask, nsk, ovk);
        auto exp_sk = sk.expanded_spending_key();
        BOOST_CHECK(exp_sk == exp_sk_2);

        // Check that full viewing key derived from sk and expanded sk are the same
        auto full_viewing_key = sk.full_viewing_key();
        BOOST_CHECK(full_viewing_key == exp_sk.full_viewing_key());

        // Check that full viewing key from primitives and from sk are the same
        auto full_viewing_key_2 = libzcash::SaplingFullViewingKey(ak, nk, ovk);
        BOOST_CHECK(full_viewing_key == full_viewing_key_2);

        // Check that incoming viewing key from primitives and from sk are the same
        auto in_viewing_key = full_viewing_key.in_viewing_key();
        auto in_viewing_key_2 = libzcash::SaplingIncomingViewingKey(ivk);
        BOOST_CHECK(in_viewing_key == in_viewing_key_2);

        // Check that the default address from primitives and from sk method are the same
        auto default_addr = sk.default_address();
        auto addrOpt2 = in_viewing_key.address(default_d);
        BOOST_CHECK(addrOpt2);
        auto default_addr_2 = addrOpt2.value();
        BOOST_CHECK(default_addr == default_addr_2);

        auto default_addr_3 = libzcash::SaplingPaymentAddress(default_d, default_pk_d);
        BOOST_CHECK(default_addr_2 == default_addr_3);
        BOOST_CHECK(default_addr == default_addr_3);
    }
}

BOOST_AUTO_TEST_CASE(StoreAndRetrieveSaplingSpendingKey) {
    CBasicKeyStore keyStore;
    libzcash::SaplingExtendedSpendingKey skOut;
    libzcash::SaplingExtendedFullViewingKey extfvkOut;
    libzcash::SaplingIncomingViewingKey ivkOut;

    std::vector<unsigned char, secure_allocator<unsigned char>> rawSeed(32);
    HDSeed seed(rawSeed);
    auto sk = libzcash::SaplingExtendedSpendingKey::Master(seed);
    auto extfvk = sk.ToXFVK();
    auto ivk = extfvk.fvk.in_viewing_key();
    auto addr = sk.DefaultAddress();

    // Sanity-check: we can't get a key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingSpendingKey(extfvk));
    BOOST_CHECK(!keyStore.GetSaplingSpendingKey(extfvk, skOut));
    // Sanity-check: we can't get a full viewing key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingFullViewingKey(ivk));
    BOOST_CHECK(!keyStore.GetSaplingFullViewingKey(ivk, extfvkOut));
    // Sanity-check: we can't get an incoming viewing key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingIncomingViewingKey(addr));
    BOOST_CHECK(!keyStore.GetSaplingIncomingViewingKey(addr, ivkOut));

    // When we specify the default address, we get the full mapping
    keyStore.AddSaplingSpendingKey(sk);
    BOOST_CHECK(keyStore.HaveSaplingSpendingKey(extfvk));
    BOOST_CHECK(keyStore.GetSaplingSpendingKey(extfvk, skOut));
    BOOST_CHECK(keyStore.HaveSaplingFullViewingKey(ivk));
    BOOST_CHECK(keyStore.GetSaplingFullViewingKey(ivk, extfvkOut));
    BOOST_CHECK(keyStore.HaveSaplingIncomingViewingKey(addr));
    BOOST_CHECK(keyStore.GetSaplingIncomingViewingKey(addr, ivkOut));
    BOOST_CHECK(sk == skOut);
    BOOST_CHECK(extfvk == extfvkOut);
    BOOST_CHECK(ivk == ivkOut);
}

BOOST_AUTO_TEST_CASE(StoreAndRetrieveSaplingFullViewingKey) {
    CBasicKeyStore keyStore;
    libzcash::SaplingExtendedSpendingKey skOut;
    libzcash::SaplingExtendedFullViewingKey extfvkOut;
    libzcash::SaplingIncomingViewingKey ivkOut;

    auto sk = GetTestMasterSaplingSpendingKey();
    auto extfvk = sk.ToXFVK();
    auto ivk = extfvk.fvk.in_viewing_key();
    auto addr = sk.DefaultAddress();

    // Sanity-check: we can't get a full viewing key we haven't added
    BOOST_CHECK(!keyStore.HaveSaplingFullViewingKey(ivk));
    BOOST_CHECK(!keyStore.GetSaplingFullViewingKey(ivk, extfvkOut));

    // and we shouldn't have a spending key or incoming viewing key either
    BOOST_CHECK(!keyStore.HaveSaplingSpendingKey(extfvk));
    BOOST_CHECK(!keyStore.GetSaplingSpendingKey(extfvk, skOut));
    BOOST_CHECK(!keyStore.HaveSaplingIncomingViewingKey(addr));
    BOOST_CHECK(!keyStore.GetSaplingIncomingViewingKey(addr, ivkOut));

    // and we can't find the default address in our list of addresses
    std::set<libzcash::SaplingPaymentAddress> addresses;
    keyStore.GetSaplingPaymentAddresses(addresses);
    BOOST_CHECK(!addresses.count(addr));

    // When we add the full viewing key, we should have it
    keyStore.AddSaplingFullViewingKey(extfvk);
    BOOST_CHECK(keyStore.HaveSaplingFullViewingKey(ivk));
    BOOST_CHECK(keyStore.GetSaplingFullViewingKey(ivk, extfvkOut));
    BOOST_CHECK(extfvk == extfvkOut);

    // We should still not have the spending key...
    BOOST_CHECK(!keyStore.HaveSaplingSpendingKey(extfvk));
    BOOST_CHECK(!keyStore.GetSaplingSpendingKey(extfvk, skOut));

    // ... but we should have an incoming viewing key
    BOOST_CHECK(keyStore.HaveSaplingIncomingViewingKey(addr));
    BOOST_CHECK(keyStore.GetSaplingIncomingViewingKey(addr, ivkOut));
    BOOST_CHECK(ivk == ivkOut);

    // ... and we should find the default address in our list of addresses
    addresses.clear();
    keyStore.GetSaplingPaymentAddresses(addresses);
    BOOST_CHECK(addresses.count(addr));
}

BOOST_AUTO_TEST_SUITE_END()