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

#ifndef FUSION_INTERPOLATION_0980078A_D871_4284_8FC6_15D2E4A59BCC
#define FUSION_INTERPOLATION_0980078A_D871_4284_8FC6_15D2E4A59BCC

//----------------------------------------------------------------------------
//
// Value Order Convention
// ----------------------
//
// 1D
// ~~
//
//   +----------+
//   x0         x1
//
//   values = [[x0],
//             [x1]
//            ]
//
//
// 2D
// ~~
//
//   x0,y1      x1,y1
//     +----------+
//     |          |
//     |          |
//     |          |
//     |          |
//     +----------+
//   x0,y0      x1,y0
//
//   values = [[x0, y0],
//             [x0, y1],
//             [x1, y0],
//             [x1, y1]
//            ]
//
//
// 3D
// ~~
//
//                x0,y1,z1            x1,y1,z1
//                   +-------------------+
//                  /|                  /|
//                 / |                 / |
//                /  |                /  |
//               /   |               /   |
//              /    |              /    |
//    x0,y0,z1 +-------------------+ x1,y0,z1
//             |     |             |     |
//             |     +-------------|-----+
//             |    / x0,y1,z0     |    / x1,y1,z0
//             |   /               |   /
//             |  /                |  /
//             | /                 | /
// z  y        |/                  |/
// | /         +-------------------+
// |/       x0,y0,z0            x1,y0,z0
// +--x
//
//   values = [[x0, y0, z0],
//             [x0, y0, z1],
//             [x0, y1, z0],
//             [x0, y1, z1],
//             [x1, y0, z0],
//             [x1, y0, z1],
//             [x1, y1, z0],
//             [x1, y1, z1]
//            ]
//
// ----
//
// And so on for higher dimensions.
//
//----------------------------------------------------------------------------

#include <array>

#include <armadillo> // v6.500+

namespace fusion_interpolation
{

//----------------------------------------------------------------------------
constexpr uint64_t power_of_two(const uint64_t exponent)
{
    return static_cast<uint64_t>(1) << exponent;
}

//----------------------------------------------------------------------------
template<uint64_t N>
using point_type = std::array<double, N>;

template<uint64_t N>
using limits_type = std::array<std::array<double, 2>, N>;

template<uint64_t N>
using point_array_type = std::array<point_type<N>, power_of_two(N)>;

template<uint64_t N>
using value_array_type = std::array<double, power_of_two(N)>;

template<uint64_t N>
using row_type = std::array<bool, N>;

template<uint64_t N>
using row_list_type = std::array<row_type<N>, power_of_two(N)>;

//----------------------------------------------------------------------------
template<uint64_t N>
row_list_type<N> multipliers()
{
    row_list_type<N> rows = {};
    for (uint64_t i(0); i < rows.size(); ++i)
    {
        for (uint64_t j(0); j < rows[i].size(); ++j)
        {
            rows[i][j] = i & power_of_two(j);
        }
    }
    return rows;
}

//----------------------------------------------------------------------------
template<uint64_t N>
point_array_type<N> points_from_limits(const limits_type<N> & limits)
{
    point_type<N> p0 = {};
    point_type<N> p1 = {};
    for (uint64_t i(0); i < N; ++i)
    {
        p0[i] = limits[i][0];
        p1[i] = limits[i][1];
    }

    point_array_type<N> pa;
    const auto two_to_N = power_of_two(N);
    for (uint64_t i(0); i < two_to_N; ++i)
    {
        for (uint64_t j(0); j < N; ++j)
        {
            pa[i][j] = (i / (two_to_N / power_of_two(j + 1)) % 2 == 0) ? p0[j] : p1[j];
        }
    }
    return pa;
}
//----------------------------------------------------------------------------
template<uint64_t N>
class regular_cell
{
public:
    regular_cell
    (
        const limits_type<N>      & limits
    ,   const value_array_type<N> & values
    )
    {
        const point_array_type<N> pa = points_from_limits(limits);

        arma::mat A(two_to_N_, two_to_N_);
        for (uint64_t i(0); i < two_to_N_; ++i)
        {
            for (uint64_t j(0); j < two_to_N_; ++j)
            {
                double v = 1.0;
                for (uint64_t k(0); k < N; ++k)
                {
                    v *= filter_[j][k] ? pa[i][k] : 1.0;
                }
                A(i, j) = v;
            }
        }
#ifdef _DEBUG
        A.print("A: ");
#endif

        arma::vec b(two_to_N_);
        for (uint64_t i(0); i < two_to_N_; ++i)
        {
            b(i) = values[i];
        }
#ifdef _DEBUG
        b.print("b: ");
#endif

        a_ = arma::solve(A, b, arma::solve_opts::equilibrate + arma::solve_opts::no_approx);
#ifdef _DEBUG
        a_.print("a: ");
#endif
    }

    double interpolate(const point_type<N> & p)
    {
        double result = 0.0;
        for (uint64_t i(0); i < two_to_N_; ++i)
        {
            double v = a_[i];
            for (uint64_t j(0); j < N; ++j)
            {
                v *= filter_[i][j] ? p[j] : 1.0;
            }
            result += v;
        }

        return result;
    }

private:

    arma::vec a_;

private:

    // These variables apply to an entire mesh so don't need to be stored on a
    // per cell basis.  If a mesh class were available they could be moved to
    // that.
    static const uint64_t two_to_N_;
    static const row_list_type<N> filter_;
};

//----------------------------------------------------------------------------
template<uint64_t N>
const uint64_t regular_cell<N>::two_to_N_ = power_of_two(N);

template<uint64_t N>
const row_list_type<N> regular_cell<N>::filter_ = multipliers<N>();

} // namespace fusion_interpolation

#endif // FUSION_INTERPOLATION_0980078A_D871_4284_8FC6_15D2E4A59BCC


///////////////////////////////////////////////////////////////////////////////
/// \file infinity.hpp
/// A time series that uses characteristic storage
//
//  Copyright 2006 Eric Niebler. 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_RANGE_RUN_STORAGE_UTILITY_INFINITY_HPP_EAN_07_21_2006
#define BOOST_RANGE_RUN_STORAGE_UTILITY_INFINITY_HPP_EAN_07_21_2006

#include <limits>
#include <boost/utility/enable_if.hpp>
#include <boost/integer_traits.hpp>
#include <boost/detail/pod_singleton.hpp>
#include <boost/type_traits/is_integral.hpp>

namespace boost { namespace range_run_storage
{
    namespace detail
    {
        template<typename T>
        typename enable_if<is_integral<T>, T>::type
        infinity(T * = 0)
        {
            return integer_traits<T>::const_max;
        }

        template<typename T>
        typename disable_if<is_integral<T>, T>::type
        infinity(T * = 0)
        {
            return std::numeric_limits<T>::infinity();
        }

        template<typename T>
        typename enable_if<is_integral<T>, T>::type
        neg_infinity(T * = 0)
        {
            return integer_traits<T>::const_min;
        }

        template<typename T>
        typename disable_if<is_integral<T>, T>::type
        neg_infinity(T * = 0)
        {
            return -std::numeric_limits<T>::infinity();
        }
    }

    template<bool Positive>
    struct infinity_base
    {
#if BOOST_WORKAROUND(BOOST_MSVC, <= 1310)
        operator std::ptrdiff_t () const    { return integer_traits<std::ptrdiff_t>::const_max; }
        operator std::size_t () const       { return integer_traits<std::size_t>::const_max; }
        operator float () const             { return std::numeric_limits<float>::infinity(); }
        operator double () const            { return std::numeric_limits<double>::infinity(); }
#else
        template<typename T>
        operator T () const
        {
            return detail::infinity<T>();
        }
#endif
    };

    template<>
    struct infinity_base<false>
    {
#if BOOST_WORKAROUND(BOOST_MSVC, <= 1310)
        operator std::ptrdiff_t () const    { return integer_traits<std::ptrdiff_t>::const_min; }
        operator std::size_t () const       { return integer_traits<std::size_t>::const_min; }
        operator float () const             { return -std::numeric_limits<float>::infinity(); }
        operator double () const            { return -std::numeric_limits<double>::infinity(); }
#else
        template<typename T>
        operator T () const
        {
            return detail::neg_infinity<T>();
        }
#endif
    };

    template<bool Positive = true>
    struct infinity
      : infinity_base<Positive>
    {
        template<typename T> friend bool operator ==(infinity inf, T t)
        {
            return static_cast<T>(inf) == t;
        }

        template<typename T> friend bool operator !=(infinity inf, T t)
        {
            return static_cast<T>(inf) != t;
        }

        template<typename T> friend bool operator ==(T t, infinity inf)
        {
            return t == static_cast<T>(inf);
        }

        template<typename T> friend bool operator !=(T t, infinity inf)
        {
            return t != static_cast<T>(inf);
        }

        infinity<!Positive> operator-() const
        {
            return infinity<!Positive>();
        }

        template<typename T> friend T &operator +=(T &t, infinity inf)
        {
            return t = static_cast<T>(inf);
        }

        template<typename T> friend T &operator -=(T &t, infinity inf)
        {
            return t = static_cast<T>(-inf);
        }

        template<typename T> friend infinity operator +(T t, infinity inf)
        {
            return inf;
        }

        template<typename T> friend infinity<!Positive> operator -(T t, infinity inf)
        {
            return -inf;
        }

        template<typename T> friend infinity operator +(infinity inf, T t)
        {
            return inf;
        }

        template<typename T> friend infinity operator -(infinity inf, T t)
        {
            return inf;
        }
    };

    typedef infinity<true> plus_infinity;
    typedef infinity<false> minus_infinity;

    namespace
    {
        plus_infinity const &inf = boost::detail::pod_singleton<plus_infinity>::instance;
    }

}}

#endif


/**
 * \copyright
 * Copyright (c) 2012-2017, OpenGeoSys Community (http://www.opengeosys.org)
 *            Distributed under a Modified BSD License.
 *              See accompanying file LICENSE.txt or
 *              http://www.opengeosys.org/project/license
 *
 */

#include <gtest/gtest.h>

#include <vector>
#include <cmath>

#include <Eigen/Eigen>

#include "MeshLib/ElementCoordinatesMappingLocal.h"
#include "MeshLib/CoordinateSystem.h"

#include "NumLib/Fem/CoordinatesMapping/ShapeMatrices.h"
#include "NumLib/Fem/FiniteElement/C0IsoparametricElements.h"
#include "NumLib/Fem/Integration/GaussIntegrationPolicy.h"
#include "NumLib/Fem/ShapeMatrixPolicy.h"

#include "FeTestData/TestFeLINE2.h"
#include "FeTestData/TestFeLINE2Y.h"
#include "FeTestData/TestFeLINE3.h"
#include "FeTestData/TestFeTRI3.h"
#include "FeTestData/TestFeQUAD4.h"
#include "FeTestData/TestFeHEX8.h"
#include "FeTestData/TestFeTET4.h"
#include "FeTestData/TestFePRISM6.h"
#include "FeTestData/TestFePYRA5.h"

#include "Tests/TestTools.h"

using namespace NumLib;
using namespace FeTestData;

namespace
{

// test cases
template <class TestFeType_, template <typename, unsigned> class ShapeMatrixPolicy_>
struct TestCase
{
    typedef TestFeType_ TestFeType;
    static const unsigned GlobalDim = TestFeType::global_dim;
    using ShapeMatrixTypes = ShapeMatrixPolicy_<typename TestFeType::ShapeFunction, GlobalDim>;
    template <typename X>
    using ShapeMatrixPolicy = ShapeMatrixPolicy_<X, GlobalDim>;
};

typedef ::testing::Types<
    TestCase<TestFeHEX8, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFeLINE2, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFeLINE2Y, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFeLINE3, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFePRISM6, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFePYRA5, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFeQUAD4, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFeTET4, EigenDynamicShapeMatrixPolicy>,
    TestCase<TestFeTRI3, EigenDynamicShapeMatrixPolicy>,

    TestCase<TestFeHEX8, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFeLINE2, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFeLINE2Y, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFeLINE3, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFePRISM6, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFePYRA5, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFeQUAD4, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFeTET4, EigenFixedShapeMatrixPolicy>,
    TestCase<TestFeTRI3, EigenFixedShapeMatrixPolicy>
    > TestTypes;
}

template <class T>
class NumLibFemIsoTest : public ::testing::Test, public T::TestFeType
{
 public:
    typedef typename T::ShapeMatrixTypes ShapeMatrixTypes;
    typedef typename T::TestFeType TestFeType;
    // Matrix types
    typedef typename ShapeMatrixTypes::NodalMatrixType NodalMatrix;
    typedef typename ShapeMatrixTypes::NodalVectorType NodalVector;
    typedef typename ShapeMatrixTypes::DimNodalMatrixType DimNodalMatrix;
    typedef typename ShapeMatrixTypes::DimMatrixType DimMatrix;
    typedef typename ShapeMatrixTypes::GlobalDimMatrixType GlobalDimMatrixType;

    // Finite element type
    template <typename X>
    using ShapeMatrixPolicy = typename T::template ShapeMatrixPolicy<X>;
    typedef typename TestFeType::template FeType<ShapeMatrixPolicy>::type FeType;

    // Shape matrix data type
    typedef typename ShapeMatrixTypes::ShapeMatrices ShapeMatricesType;
    typedef typename TestFeType::MeshElementType MeshElementType;

    static const unsigned dim = TestFeType::dim;
    static const unsigned e_nnodes = TestFeType::e_nnodes;
    static const unsigned n_sample_pt_order2 = TestFeType::n_sample_pt_order2;
    static const unsigned n_sample_pt_order3 = TestFeType::n_sample_pt_order3;

    using IntegrationMethod =
        typename NumLib::GaussIntegrationPolicy<MeshElementType>::IntegrationMethod;

 public:
    NumLibFemIsoTest() :
        D(dim, dim),
        expectedM(e_nnodes,e_nnodes),
        expectedK(e_nnodes,e_nnodes),
        integration_method(2)
    {
        // create a mesh element used for testing
        mesh_element = this->createMeshElement();

        // set a conductivity tensor
        setIdentityMatrix(dim, D);
        D *= conductivity;
        MeshLib::ElementCoordinatesMappingLocal ele_local_coord(
            *mesh_element, MeshLib::CoordinateSystem(*mesh_element).getDimension());
        auto R = ele_local_coord.getRotationMatrixToGlobal().topLeftCorner(
            TestFeType::dim, TestFeType::global_dim);
        globalD.noalias() = R.transpose() * (D * R);

        // set expected matrices
        this->setExpectedMassMatrix(expectedM);
        this->setExpectedLaplaceMatrix(conductivity, expectedK);

        // for destructor
        vec_eles.push_back(mesh_element);
        for (auto e : vec_eles)
            for (unsigned i=0; i<e->getNumberOfNodes(); i++)
                vec_nodes.push_back(e->getNode(i));
    }

    virtual ~NumLibFemIsoTest()
    {
        for (auto itr = vec_nodes.begin(); itr!=vec_nodes.end(); ++itr )
            delete *itr;
        for (auto itr = vec_eles.begin(); itr!=vec_eles.end(); ++itr )
            delete *itr;
    }

    static const double conductivity;
    static const double eps;
    DimMatrix D;
    NodalMatrix expectedM;
    NodalMatrix expectedK;
    IntegrationMethod integration_method;
    GlobalDimMatrixType globalD;

    std::vector<const MeshLib::Node*> vec_nodes;
    std::vector<const MeshElementType*> vec_eles;
    MeshElementType* mesh_element;

 public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
}; // NumLibFemIsoTest

template <class T>
const double NumLibFemIsoTest<T>::conductivity = 1e-11;

template <class T>
const double NumLibFemIsoTest<T>::eps = 2*std::numeric_limits<double>::epsilon();

template <class T>
const unsigned NumLibFemIsoTest<T>::dim;

template <class T>
const unsigned NumLibFemIsoTest<T>::e_nnodes;

template <class T>
const unsigned NumLibFemIsoTest<T>::n_sample_pt_order2;

template <class T>
const unsigned NumLibFemIsoTest<T>::n_sample_pt_order3;

TYPED_TEST_CASE(NumLibFemIsoTest, TestTypes);

TYPED_TEST(NumLibFemIsoTest, CheckMassMatrix)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeType FeType;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object
    FeType fe(*this->mesh_element);

    // evaluate a mass matrix M = int{ N^T D N }dA_e
    NodalMatrix M(this->e_nnodes, this->e_nnodes);
    M.setZero();
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    for (std::size_t i=0; i < this->integration_method.getNumberOfPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.template computeShapeFunctions<ShapeMatrixType::N_J>(
            wp.getCoords(), shape, this->global_dim, false);
        M.noalias() += shape.N.transpose() * shape.N * shape.detJ * wp.getWeight();
    }
    //std::cout << "M=\n" << M;
    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);
}

TYPED_TEST(NumLibFemIsoTest, CheckLaplaceMatrix)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeType FeType;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object
    FeType fe(*this->mesh_element);

    // evaluate a Laplace matrix K = int{ dNdx^T D dNdx }dA_e
    NodalMatrix K(this->e_nnodes, this->e_nnodes);
    K.setZero();
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    for (std::size_t i=0; i < this->integration_method.getNumberOfPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.template computeShapeFunctions<ShapeMatrixType::DNDX>(
            wp.getCoords(), shape, this->global_dim, false);
        K.noalias() += shape.dNdx.transpose() * this->globalD * shape.dNdx * shape.detJ * wp.getWeight();
    }
    //std::cout << "K=\n" << K << std::endl;
    ASSERT_ARRAY_NEAR(this->expectedK.data(), K.data(), K.size(), this->eps);
}

TYPED_TEST(NumLibFemIsoTest, CheckMassLaplaceMatrices)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeType FeType;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object
    FeType fe(*this->mesh_element);

    // evaluate both mass and laplace matrices at once
    NodalMatrix M(this->e_nnodes, this->e_nnodes);
    M.setZero();
    NodalMatrix K(this->e_nnodes, this->e_nnodes);
    K.setZero();
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    for (std::size_t i=0; i < this->integration_method.getNumberOfPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.computeShapeFunctions(wp.getCoords(), shape, this->global_dim, false);
        M.noalias() += shape.N.transpose() * shape.N * shape.detJ * wp.getWeight();
        K.noalias() += shape.dNdx.transpose() * (this->globalD * shape.dNdx) * shape.detJ * wp.getWeight();
    }

    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);
    ASSERT_ARRAY_NEAR(this->expectedK.data(), K.data(), K.size(), this->eps);
}

#if 0
TYPED_TEST(NumLibFemIsoTest, CheckGaussIntegrationLevel)
{
    // Refer to typedefs in the fixture
    typedef typename TestFixture::FeType FeType;
    typedef typename TestFixture::NodalMatrix NodalMatrix;
    typedef typename TestFixture::ShapeMatricesType ShapeMatricesType;

    // create a finite element object with gauss quadrature level 2
    FeType fe(*this->mesh_element);

    // evaluate a mass matrix
    NodalMatrix M(this->e_nnodes, this->e_nnodes);
    M.setZero();
    ShapeMatricesType shape(this->dim, this->global_dim, this->e_nnodes);
    ASSERT_EQ(TestFixture::n_sample_pt_order2, this->integration_method.getNumberOfPoints());
    for (std::size_t i=0; i < this->integration_method.getNumberOfPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.computeShapeFunctions(wp.getCoords(), shape);
        M.noalias() += shape.N * shape.N.transpose() * shape.detJ * wp.getWeight();
    }
    //std::cout << "M=\n" << M << std::endl;
    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);

    // Change gauss quadrature level to 3
    this->integration_method.setIntegrationOrder(3);
    M *= .0;
    ASSERT_EQ(TestFixture::n_sample_pt_order3, this->integration_method.getNumberOfPoints());
    for (std::size_t i=0; i < this->integration_method.getNumberOfPoints(); i++) {
        shape.setZero();
        auto wp = this->integration_method.getWeightedPoint(i);
        fe.computeShapeFunctions(wp.getCoords(), shape);
        M.noalias() += shape.N * shape.N.transpose() * shape.detJ * wp.getWeight();
    }
    //std::cout << "M=\n" << M << std::endl;
    ASSERT_ARRAY_NEAR(this->expectedM.data(), M.data(), M.size(), this->eps);
}
#endif


// Copyright Louis Dionne 2015
// Distributed under the Boost Software License, Version 1.0.

#include <boost/hana.hpp>

#include <cstddef>
using namespace boost::hana;
using namespace boost::hana::literals;

// sample(indexed_sort-now-impl)
auto indexed_sort = [](auto list, auto predicate) {
  auto indices = to<Tuple>(range(0_c, size(list)));
  auto indexed_list = zip.with(make_pair, list, indices);
  auto sorted = sort(indexed_list, [&](auto const& x, auto const& y) {
    return predicate(first(x), first(y));
  });
  return make_pair(transform(sorted, first), transform(sorted, second));
};
// end-sample

int main() {
// sample(indexed_sort-now-usage1)
auto types = tuple_t<int[3], int[2], int[1]>;
auto indexed = indexed_sort(types, [](auto t, auto u) {
    return sizeof_(t) < sizeof_(u);
});

auto sorted = first(indexed);
auto indices = second(indexed);

static_assert(sorted == tuple_t<int[1], int[2], int[3]>, "");
static_assert(indices == tuple_c<std::size_t, 2, 1, 0>, "");
// end-sample

// sample(indexed_sort-now-usage2)
using Sequence = decltype(unpack(sorted, template_<_tuple>))::type;
auto index_map = second(indexed_sort(indices, less));

Sequence s;
int (&a)[3] = s[index_map[0_c]];
int (&b)[2] = s[index_map[1_c]];
int (&c)[1] = s[index_map[2_c]];
// end-sample

(void)a;
(void)b;
(void)c;
}


#include <iostream>
#include <vector>
#include <list>
#include <set>
#include <tuple>
#include <unordered_map>
#include <cstdio>
#include <utility>
#include <cstdlib>
#include <map>
#include <cmath>
#include <ctgmath>
#include <cstring>
#include <functional>
#include <cassert>
#include <algorithm>
#include <unordered_map>
#include <fstream>
#include <stdio.h>

#include <boost/algorithm/string.hpp>
#include <Eigen/Dense>
#include "parse_args.hh"
#include "omp.h"

#define KNRM  "\x1B[0m"
#define KRED  "\x1B[31m"
#define KGRN  "\x1B[32m"
#define KYEL  "\x1B[33m"
#define KBLU  "\x1B[34m"
#define KMAG  "\x1B[35m"
#define KCYN  "\x1B[36m"
#define KWHT  "\x1B[37m"

using namespace std;
using namespace Eigen;

int D; // dimensionality

unordered_map<string, VectorXd> w; // target word vectors
unordered_map<string, VectorXd> c; // context word vectors
unordered_map<string, double> bw; // target word biases
unordered_map<string, double> bc; // context word biases

unordered_map<string, VectorXd> grad_w; // Squared gradient for AdaGrad
unordered_map<string, VectorXd> grad_c;
unordered_map<string, double> grad_bw;
unordered_map<string, double> grad_bc;

struct edge {
  string w, c;
  double value;
};
vector<edge> edges;

set<string> words;
set<string> contexts;

unordered_map<string, double> pairs;

void read_edges(string fname, vector<edge> &train_data){
    ifstream train_file(fname.c_str());
    string first, second;
    double value;
    while (train_file >> first >> second >> value){
        edge e;
        e.w = first;
        e.c = second;
        e.value = value;
        //cout << e.w << "\t" << e.c << "\t" << e.value << endl;
        words.insert(first);
        contexts.insert(second);
        train_data.push_back(e);
    }
    train_file.close();
    // allocate memory for all vectors
}

void load_word_vectors(string vects_fname){
    // Initialize word vectors using pre-trained word vectors
    fprintf(stderr, "%sDimensionality of the word vectors = %d%s\n", KRED, D, KNRM);
    fprintf(stderr, "%sReading pre-trained vectors from %s%s\n", KRED, vects_fname.c_str(), KNRM);
    unordered_map<string, VectorXd> x;
    set<string> prewords;

    FILE *fp= fopen(vects_fname.c_str(), "r");
    int count = 0;
    for (char buf[262144]; fgets(buf, sizeof(buf), fp); ) {
        char curword[1024];
        sscanf(buf, "%s %[^\n]", curword, buf);
        string wstr(curword);
        if ((words.find(wstr) != words.end()) || (contexts.find(wstr) != contexts.end())){
            x[wstr] = VectorXd::Zero(D);
            count = 0;
            while (1) {
                double fval;
                bool end = (sscanf(buf, "%lf %[^\n]", &fval, buf) != 2);
                x[wstr][count] = fval;
                count += 1;
            if (end) break;
            }
            prewords.insert(wstr);
            assert(count == D);
        }
    }
    // Copy the vectors from x to w and c.
    for (auto y = x.begin(); y != x.end(); ++y){
        if (words.find(y->first) != words.end())
            w[y->first] = y->second;
        if (contexts.find(y->first) != contexts.end())
            c[y->first] = y->second;
    }
    fclose(fp);
}

void read_pairs(string pairs_fname){
    fprintf(stderr, "%sReading relational pairs from = %s%s\n", KRED, pairs_fname.c_str(), KNRM);
    ifstream pairs_file(pairs_fname.c_str());
    string first_word, second_word;
    double value;

    while (pairs_file >> first_word >> second_word >> value){
        //cout << first_word << "\t" << second_word << "\t" << value << "\t" << (2 * value) << endl;
        pairs[first_word + "<+>" + second_word] = value;
    }

    /*
    while (pairs_file >> first_word >> second_word){
        //cout << first_word << "\t" << second_word << "\t" << value << endl;
        pairs[first_word + "<+>" + second_word] = 1.0;
    }
    */

    pairs_file.close();
}

void centralize(unordered_map<string, VectorXd> &x){
    VectorXd mean = VectorXd::Zero(D);
    VectorXd squared_mean = VectorXd::Zero(D);
    for (auto w = x.begin(); w != x.end(); ++w){
        mean += w->second;
        squared_mean += (w->second).cwiseProduct(w->second);
    }
    mean = mean / ((double) x.size());
    VectorXd sd = squared_mean - mean.cwiseProduct(mean);
    for (int i = 0; i < D; ++i){
        sd[i] = sqrt(sd[i]);
    }
    for (auto w = x.begin(); w != x.end(); ++w){
        VectorXd tmp = VectorXd::Zero(D);
        for (int i = 0; i < D; ++i){
            tmp[i] = (w->second)[i] - mean[i];
            if (sd[i] != 0)
                tmp[i] /= sd[i];
        }
        w->second = tmp;
    }
}

void initialize(){
    int count_words = 0;
    for(auto e = words.begin(); e != words.end(); ++e){
        count_words++;
        w[*e] = VectorXd::Random(D);
        bw[*e] = 0;
        grad_w[*e] = VectorXd::Zero(D);
        grad_bw[*e] = 0;
    }

    int count_contexts = 0;
    for(auto e = contexts.begin(); e != contexts.end(); ++e){
        count_contexts++;
        c[*e] = VectorXd::Random(D);
        bc[*e] = 0;
        grad_c[*e] = VectorXd::Zero(D);
        grad_bc[*e] = 0;
    }

    centralize(w);
    centralize(c);
    fprintf(stderr, "%sInitialization Completed...\n%s", KYEL, KNRM);
}

double f(size_t x){
    if (x < 100)
        return pow((x / 100.0), 0.75);
    else
        return 1.0;
}

void train(int epohs, double alpha, double lambda){
    fprintf(stderr, "%s\nTotal ephos to train = %d\n%s", KGRN, epohs, KNRM);
    fprintf(stderr, "%sInitial learning rate = %f\n%s", KGRN, alpha, KNRM);
    fprintf(stderr, "%slambda = %f\n%s", KGRN, lambda, KNRM);
    fprintf(stderr, "%sDim = %d\n%s", KGRN, D, KNRM);

    double total_loss, cost;
    VectorXd gw = VectorXd::Zero(D);
    VectorXd gc = VectorXd::Zero(D);
    VectorXd diff = VectorXd::Zero(D);

    VectorXd one_vect = VectorXd::Zero(D);
    for (auto i = 0; i < D; ++i)
        one_vect[i] = 1.0;

    int found_pairs = 0;

    for (int t = 0; t < epohs; ++t){
        total_loss = 0;
        found_pairs = 0;
        for(auto e = edges.begin(); e != edges.end(); ++e){
            cost = w[e->w].dot(c[e->c]) + bw[e->w] + bc[e->c] - log(e->value);
            total_loss += f(e->value) * cost * cost;

            cost *= f(e->value);
            gw = cost * c[e->c];
            gc = cost * w[e->w];

            string pair_key = e->w + "<+>" + e->c;
            if (pairs.find(pair_key) != pairs.end()){
                diff = lambda * pairs[pair_key] * (w[e->w] - c[e->c]);
                gw += diff;
                gc -= diff;
                found_pairs++;
                }

            grad_w[e->w] += gw.cwiseProduct(gw);
            grad_c[e->c] += gc.cwiseProduct(gc);
            grad_bw[e->w] += cost * cost;
            grad_bc[e->c] += cost * cost;

            w[e->w] -= alpha * gw.cwiseProduct((grad_w[e->w] + one_vect).cwiseInverse().cwiseSqrt());
            c[e->c] -= alpha * gc.cwiseProduct((grad_c[e->c] + one_vect).cwiseInverse().cwiseSqrt());

            bw[e->w] -= (alpha * cost) / sqrt(1.0 + grad_bw[e->w]);
            bc[e->c] -= (alpha * cost) / sqrt(1.0 + grad_bc[e->c]);
        }
        fprintf(stderr, "Itr = %d, Loss = %f, foundPairs = %d\n", t, (sqrt(total_loss) / edges.size()), found_pairs);
    }
}

void write_line(ofstream &reps_file, VectorXd vec, string label){
    reps_file << label + " ";
    for (int i = 0; i < D; ++i)
        reps_file << vec[i] << " ";
    reps_file << endl;
}

void save_model(string fname){
    ofstream reps_file;
    reps_file.open(fname);
    if (!reps_file){
        fprintf(stderr, "%sFailed to write reps to %s\n%s", KRED, KNRM, fname.c_str());
        exit(1);
    }
    for (auto x = words.begin(); x != words.end(); ++x){
        if (contexts.find(*x) != contexts.end())
             write_line(reps_file, 0.5 * (w[*x] + c[*x]), *x);
        else
            write_line(reps_file, w[*x], *x);
    }

    for (auto x = contexts.begin(); x != contexts.end(); ++x){
        if (words.find(*x) == words.end())
            write_line(reps_file, c[*x], *x);
    }
    reps_file.close();
}

int main(int argc, char *argv[]){
    int no_threads = 100;
    omp_set_num_threads(no_threads);
    setNbThreads(no_threads);
    initParallel();

    if (argc == 1) {
        fprintf(stderr, "usage: ./reps --dim=dimensionality --model=model_fname \
                                --alpha=alpha --ephos=rounds --lmda=lambda --edges=edges_fname \
                                 --pretrain=pretrained_word_vectors_file (if any) \
                                 --pairs=pairs_file_name \n");
        return 0;
    }
    parse_args::init(argc, argv);
    string edges_fname = parse_args::get<string>("--edges");
    string pretrain = parse_args::get<string>("--pretrain");
    string pairs_fname = parse_args::get<string>("--pairs");

    D = parse_args::get<int>("--dim");
    int epohs = parse_args::get<int>("--epohs");
    double alpha = parse_args::get<double>("--alpha");
    string model = parse_args::get<string>("--model");
    double lambda = parse_args::get<double>("--lmda");

    read_edges(edges_fname, edges);
    fprintf(stderr, "%sTotal no. of target train instances = %d\n%s", KGRN, (int) edges.size(), KNRM);
    fprintf(stderr, "%sTotal no of words = %d\n%s", KGRN, (int) words.size(), KNRM);
    fprintf(stderr, "%sTotal no. of contexts = %d\n%s", KGRN, (int) contexts.size(), KNRM);
    read_pairs(pairs_fname);
    //test_code();
    initialize();
    if (pretrain.length() > 0){
        load_word_vectors(pretrain);
    }
    train(epohs, alpha, lambda);
    save_model(model);
    return 0;

}


/*
 * Copyright 2015-2019 CNRS-UM LIRMM, CNRS-AIST JRL
 */

#include <mc_filter/ExponentialMovingAverage.h>
#include <mc_filter/utils/clamp.h>

#include <boost/test/unit_test.hpp>

#include <Eigen/Core>

template<typename DerivedA, typename DerivedB>
bool allclose(
    const Eigen::DenseBase<DerivedA> & a,
    const Eigen::DenseBase<DerivedB> & b,
    const typename DerivedA::RealScalar & rtol = Eigen::NumTraits<typename DerivedA::RealScalar>::dummy_precision(),
    const typename DerivedA::RealScalar & atol = Eigen::NumTraits<typename DerivedA::RealScalar>::epsilon())
{
  return ((a.derived() - b.derived()).array().abs() <= (atol + rtol * b.derived().array().abs())).all();
}

BOOST_AUTO_TEST_CASE(TestExponentialMovingAverage)
{
  using namespace mc_filter;
  double dt = 0.005;
  const double T = 15;
  const double C = 100;
  size_t iter = static_cast<size_t>(ceil(T / dt));

  {
    const Eigen::Vector3d constant{C, C, C};
    ExponentialMovingAverage<Eigen::Vector3d> average(dt, T);
    for(size_t i = 0; i < iter; ++i)
    {
      average.append(constant);
    }

    // Check that averaging a constant for the characteristic time T gives the
    // expected theoratical value 1-1/e
    Eigen::Vector3d expected = C * (1 - std::exp(-1)) * Eigen::Vector3d::Ones();
    BOOST_CHECK(allclose(average.eval(), expected, 1e-10));
  }

  {
    using Vector6d = Eigen::Matrix<double, 6, 1>;
    const Vector6d constant = C * Vector6d::Ones();
    ExponentialMovingAverage<Vector6d> average(dt, T);
    for(size_t i = 0; i < iter; ++i)
    {
      average.append(constant);
    }

    // Check that averaging a constant for the characteristic time T gives the
    // expected theoratical value 1-1/e
    Vector6d expected = C * (1 - std::exp(-1)) * Vector6d::Ones();
    BOOST_CHECK(allclose(average.eval(), expected, 1e-10));
  }
}

BOOST_AUTO_TEST_CASE(test_clamp)
{
  using namespace mc_filter;
  double min = -0.1;
  double max = 0.1;
  BOOST_REQUIRE_CLOSE(utils::clamp(0.5, min, max), max, 1e-10);
  BOOST_REQUIRE_CLOSE(utils::clamp(-0.5, min, max), min, 1e-10);
  BOOST_REQUIRE_CLOSE(utils::clampAndWarn(0.5, min, max, ""), max, 1e-10);
  BOOST_REQUIRE_CLOSE(utils::clampAndWarn(-0.5, min, max, ""), min, 1e-10);
  {
    double val = 0.5;
    utils::clampInPlace(val, min, max);
    BOOST_REQUIRE_CLOSE(val, max, 1e-10);
    val = -0.5;
    utils::clampInPlace(val, min, max);
    BOOST_REQUIRE_CLOSE(val, min, 1e-10);
  }
  {
    double val = 0.5;
    utils::clampInPlace(val, min, max);
    BOOST_REQUIRE_CLOSE(val, max, 1e-10);
    val = -0.5;
    utils::clampInPlace(val, min, max);
    BOOST_REQUIRE_CLOSE(val, min, 1e-10);
  }
  {
    double val = 0.5;
    utils::clampInPlaceAndWarn(val, min, max, "Value");
    BOOST_REQUIRE_CLOSE(val, max, 1e-10);
    val = -0.5;
    utils::clampInPlaceAndWarn(val, min, max, "Value");
    BOOST_REQUIRE_CLOSE(val, min, 1e-10);
  }
  {
    double val = 0.5;
    utils::clampInPlaceAndWarn(val, min, max, "Value");
    BOOST_REQUIRE_CLOSE(val, max, 1e-10);
    val = -0.5;
    utils::clampInPlaceAndWarn(val, min, max, "Value");
    BOOST_REQUIRE_CLOSE(val, min, 1e-10);
  }

  // Vector version
  {
    Eigen::Vector3d minV{-1, -2, -3};
    Eigen::Vector3d maxV{1, 2, 3};
    Eigen::Vector3d valMax = 10 * maxV;
    Eigen::Vector3d valMin = 10 * minV;
    BOOST_CHECK(allclose(utils::clamp(valMax, -1., 1.), 1. * Eigen::Vector3d::Ones()));
    BOOST_CHECK(allclose(utils::clamp(valMin, -1., 1.), -1. * Eigen::Vector3d::Ones()));
    utils::clampInPlace(valMax, -1., 1.);
    BOOST_CHECK(allclose(valMax, 1. * Eigen::Vector3d::Ones()));
    utils::clampInPlace(valMin, -1., 1.);
    BOOST_CHECK(allclose(valMin, -1. * Eigen::Vector3d::Ones()));
    valMax = 10 * maxV;
    valMin = 10 * minV;
    BOOST_CHECK(allclose(utils::clamp(valMax, minV, maxV), maxV));
    BOOST_CHECK(allclose(utils::clamp(valMin, minV, maxV), minV));
    utils::clampInPlace(valMax, minV, maxV);
    BOOST_CHECK(allclose(valMax, maxV));
    utils::clampInPlace(valMin, minV, maxV);
    BOOST_CHECK(allclose(valMin, minV));
  }
  // Vector version with warning log
  {
    Eigen::Vector3d minV{-1, -2, -3};
    Eigen::Vector3d maxV{1, 2, 3};
    Eigen::Vector3d valMax = 10 * Eigen::Vector3d::Ones();
    Eigen::Vector3d valMin = -valMax;
    BOOST_CHECK(allclose(utils::clampAndWarn(valMax, minV, maxV, "Vector"), maxV));
    BOOST_CHECK(allclose(utils::clampAndWarn(valMin, minV, maxV, "Vector"), minV));
    utils::clampInPlaceAndWarn(valMax, minV, maxV, "Vector");
    BOOST_CHECK(allclose(valMax, maxV));
    utils::clampInPlaceAndWarn(valMin, minV, maxV, "Vector");
    BOOST_CHECK(allclose(valMin, minV));
  }
}


/*
 * Geometry.hpp
 *
 *  Created on: Mar 19, 2020
 *      Author: jelavice
 */

#pragma once

#include <Eigen/Dense>
#include <utility>
#include "pure_pursuit_core/Path.hpp"

namespace pure_pursuit {

class RobotState;
class RobotPose;

struct Line {
  Line() = default;
  Line(const Point& p1, const Point& p2);
  Line(double x1, double y1, double x2, double y2);
  /*
   * Line is defined with two points
   */
  Point p1_, p2_;
};

struct Circle {
  Circle() = default;
  Circle(const Point& c, double r);
  Circle(double x, double y, double r);
  Point center_{0.0, 0.0};  // center
  double r_ = 1.0;          // radius
};

struct Intersection {
  enum class SolutionCase : int { NO_SOLUTION, ONE_SOLUTION, TWO_SOLUTIONS };
  Point p1_, p2_;
  SolutionCase solutionCase_;
};

class RateLimiter {
 public:
  RateLimiter() = default;
  ~RateLimiter() = default;

  double limitRateOfChange(double value);
  void setRisingRate(double maxRisingRate);
  void setFallingRate(double minFallingRate);
  void setTimestep(double dt);
  void reset(double startingValue = 0.0);

 private:
  double dt_ = 0.01;
  double maxRisingRate_ = 1.0;
  double minFallingRate_ = -1.0;
  double valuePrev_ = 0.0;
};

class AverageFilter {
 public:
  AverageFilter() = default;
  ~AverageFilter() = default;

  double filterInputValue(double value);
  void setWeightForMostRecentMeasurement(double weight);

 private:
  double weightMostRecentMeasurement_ = 1.0;
  double filteredValuePrev_ = 0.0;
  bool firstTime_ = true;
};

template <typename T>
int sgn(T val) {
  return (T(0) < val) - (val < T(0));
}

double deadZone(double x, double deadzoneWidth);
double bindToRange(double value, double lo, double hi);
unsigned int bindIndexToRange(int idReq, int lo, int hi);
bool isAlmostZero(double val);
bool isClose(double val1, double val2);

double euclideanDistance(const RobotState& s1, const RobotState& s2);
double euclideanDistance(const RobotPose& p1, const RobotPose& p2);
double euclideanDistance(const RobotState& s, const PathPoint& p);

void computeIntersection(const Line& line, const Circle& circle, Intersection* intersection);
Vector computeNormalizedFinalApproachDirection(const PathSegment& pathSegment);
void appendPointAlongFinalApproachDirection(double extendingDistance, PathSegment* pathSegment);
Vector computeDesiredHeadingVector(double yawAngle, DrivingDirection desiredDrivingDirection);
Matrix rotationMatrix(double angle);
unsigned int getIdOfTheClosestPointOnThePath(const PathSegment& pathSegment, const Point& robotPosition, unsigned int lastClosestId = 0);
bool isPastLastPoint(const PathSegment& pathSegment, const Point& robPos);
void findIdsOfTwoPointsDefiningALine(const RobotState& robotState, const PathSegment& pathSegment, const Point& anchorPoint,
                                     unsigned int startingPoint, double lookaheadDistance, unsigned int* closerPointId,
                                     unsigned int* fartherPointId);
Point chooseCorrectLookaheadPoint(const Intersection& intersection, const Vector& desiredHeading, const Point& origin);
bool computeLookaheadAngle(const Point& lookaheadPoint, const Point& anchorPoint, const Vector& heading, DrivingDirection drivingDirection,
                           double* lookaheadAngle);
double computeSteeringAngleCmd(double lookaheadAngle, double lookaheadDistance, double anchorDistancte, double wheelBase);
Point computeAnchorPoint(const RobotPose& robotState, double anchorDistance, DrivingDirection drivingDirection);
bool computeLookaheadPoint(unsigned int closestPointOnPathSegmentId, double lookaheadDistance, const RobotState& robotState,
                           DrivingDirection drivingDirection, const PathSegment& pathSegment, double anchorDistance, Point* lookaheadPoint);

} /* namespace pure_pursuit */


#ifndef MPLLIBS_METAMONAD_V1_LIST_HPP
#define MPLLIBS_METAMONAD_V1_LIST_HPP

// Copyright Abel Sinkovics (abel@sinkovics.hu)  2011.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <mpllibs/metamonad/v1/list_tag.hpp>
#include <mpllibs/metamonad/v1/concat.hpp>
#include <mpllibs/metamonad/v1/tmp_tag.hpp>
#include <mpllibs/metamonad/v1/monad.hpp>
#include <mpllibs/metamonad/v1/monad_plus.hpp>
#include <mpllibs/metamonad/v1/monoid.hpp>
#include <mpllibs/metamonad/v1/lambda.hpp>
#include <mpllibs/metamonad/v1/name.hpp>
#include <mpllibs/metamonad/v1/concat_map.hpp>

#include <boost/mpl/list.hpp>

namespace mpllibs
{
  namespace metamonad
  {
    namespace v1
    {
      template <>
      struct monad<list_tag> : monad_defaults<list_tag>
      {
        typedef lambda_c<t, boost::mpl::list<t> > return_;
        typedef concat_map<> bind;
      };

      template <>
      struct monoid<list_tag> : monoid_defaults<list_tag>
      {
        typedef boost::mpl::list<> mempty;
        typedef concat<> mappend;
      };

      template <>
      struct monad_plus<list_tag>
      {
        typedef monoid<list_tag>::mempty mzero;
        typedef monoid<list_tag>::mappend mplus;
      };
    }
  }
}

#endif


/*
 * Copyright (c) 2021 The Foundation for Research on Information Technologies in Society (IT'IS).
 *
 * This file is part of iSEG
 * (see https://github.com/ITISFoundation/osparc-iseg).
 *
 * This software is released under the MIT License.
 *  https://opensource.org/licenses/MIT
 */
#include "LevelsetWidget.h"

#include "Data/ItkUtils.h"
#include "Data/Logger.h"
#include "Data/SlicesHandlerITKInterface.h"

#include <itkApproximateSignedDistanceMapImageFilter.h>
#include <itkBinaryThresholdImageFilter.h>
#include <itkConstNeighborhoodIterator.h>
#include <itkFastMarchingImageFilter.h>
#include <itkImage.h>
#include <itkThresholdSegmentationLevelSetImageFilter.h>

#include <QFormLayout>

#include <accumulators/percentile.hpp>
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/max.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/min.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/variance.hpp>

#include <algorithm>
#include <sstream>

namespace acc = boost::accumulators;

LevelsetWidget::LevelsetWidget(iseg::SlicesHandlerInterface* hand3D)
		: m_Handler3D(hand3D)
{
	m_Activeslice = m_Handler3D->ActiveSlice();

	setToolTip(Format("LevelSetSegmentation: (Pick with OLC Foreground 1 pixel to start)"));

	m_AllSlices = new QCheckBox;

	m_InitFromTarget = new QCheckBox;

	m_CurvatureScaling = new QLineEdit(QString::number(1.0));
	m_CurvatureScaling->setValidator(new QDoubleValidator);

	m_LowerThreshold = new QLineEdit(QString::number(0.0));
	m_LowerThreshold->setValidator(new QDoubleValidator);

	m_UpperThreshold = new QLineEdit(QString::number(1.0));
	m_UpperThreshold->setValidator(new QDoubleValidator);

	m_EdgeWeight = new QLineEdit(QString::number(0.0));
	m_EdgeWeight->setValidator(new QDoubleValidator);

	m_Multiplier = new QLineEdit(QString::number(2.5));
	m_Multiplier->setValidator(new QDoubleValidator);
	m_Multiplier->setToolTip(Format(
			"Used to estimate thresholds."
			"The confidence interval is the mean plus or minus the 'Multiplier' times the standard deviation."));

	m_GuessThreshold = new QPushButton("Estimate thresholds");

	m_ClearSeeds = new QPushButton("Clear seeds");

	m_ExecuteButton = new QPushButton("Execute");

	auto layout = new QFormLayout;
	layout->addRow(QString("Apply to all slices"), m_AllSlices);

	layout->addRow(QString("Initialize from target"), m_InitFromTarget);
	layout->addRow(QString("Curvature scaling"), m_CurvatureScaling);
	layout->addRow(QString("Lower threshold"), m_LowerThreshold);
	layout->addRow(QString("Upper threshold"), m_UpperThreshold);
	layout->addRow(QString("Edge weight"), m_EdgeWeight);
	layout->addRow(QString("Multiplier"), m_Multiplier);
	layout->addRow(m_GuessThreshold, m_ClearSeeds);
	layout->addRow(m_ExecuteButton);
	setLayout(layout);

	QObject_connect(m_GuessThreshold, SIGNAL(clicked()), this, SLOT(GuessThresholds()));
	QObject_connect(m_ClearSeeds, SIGNAL(clicked()), this, SLOT(Clearmarks()));
	QObject_connect(m_ExecuteButton, SIGNAL(clicked()), this, SLOT(DoWork()));
}

void LevelsetWidget::Init()
{
	OnSlicenrChanged();
	HideParamsChanged();
}

void LevelsetWidget::NewLoaded()
{
	Clearmarks();
	OnSlicenrChanged();
}

void LevelsetWidget::OnSlicenrChanged()
{
	m_Activeslice = m_Handler3D->ActiveSlice();
}

void LevelsetWidget::Cleanup()
{
	Clearmarks();
}

void LevelsetWidget::Clearmarks()
{
	m_Vpdyn.clear();

	std::vector<iseg::Point> empty;
	emit VpdynChanged(&empty);
}

void LevelsetWidget::OnMouseClicked(iseg::Point p)
{
	m_Vpdyn[m_Activeslice].push_back(p);

	if (!m_AllSlices->isChecked())
	{
		//DoWork();
	}
}

void LevelsetWidget::GetSeeds(std::vector<itk::Index<2>>& seeds)
{
	auto vp = m_Vpdyn[m_Activeslice];
	for (auto p : vp)
	{
		itk::Index<2> idx;
		idx[0] = p.px;
		idx[1] = p.py;
		seeds.push_back(idx);
	}
}

void LevelsetWidget::GetSeeds(std::vector<itk::Index<3>>& seeds)
{
	auto start_slice = m_Handler3D->StartSlice();
	for (const auto& slice : m_Vpdyn)
	{
		for (auto p : slice.second)
		{
			itk::Index<3> idx;
			idx[0] = p.px;
			idx[1] = p.py;
			idx[2] = slice.first - start_slice;
			seeds.push_back(idx);
		}
	}
}

void LevelsetWidget::GuessThresholds()
{
	iseg::SlicesHandlerITKInterface itk_handler(m_Handler3D);
	if (m_AllSlices->isChecked())
	{
		using input_type = itk::SliceContiguousImage<float>;
		auto source = itk_handler.GetSource(true);
		GuessThresholdsNd<input_type>(source);
	}
	else
	{
		using input_type = itk::Image<float, 2>;
		auto source = itk_handler.GetSourceSlice();
		GuessThresholdsNd<input_type>(source);
	}
}

template<class TInput>
void LevelsetWidget::GuessThresholdsNd(TInput* source)
{
	using input_type = TInput;

	std::vector<typename input_type::IndexType> indices;
	GetSeeds(indices);

	typename input_type::SizeType radius;
	radius.Fill(2);

	acc::accumulator_set<double, acc::features<acc::tag::mean, acc::tag::variance>> stats;

	itk::ConstNeighborhoodIterator<input_type> it(radius, source, source->GetLargestPossibleRegion());
	size_t const n = it.Size();
	for (auto idx : indices)
	{
		it.SetLocation(idx);

		for (size_t i = 0; i < n; i++)
		{
			bool in_bounds;
			double v = it.GetPixel(i, in_bounds);
			if (in_bounds)
			{
				stats(v);
			}
		}
	}

	auto mean = acc::mean(stats);
	auto stddev = std::sqrt(acc::variance(stats));
	auto s = m_Multiplier->text().toDouble();
	m_LowerThreshold->setText(QString::number(mean - s * stddev));
	m_UpperThreshold->setText(QString::number(mean + s * stddev));
}

void LevelsetWidget::DoWork()
{
	iseg::SlicesHandlerITKInterface itk_handler(m_Handler3D);
	if (m_AllSlices->isChecked())
	{
		using input_type = itk::SliceContiguousImage<float>;
		auto source = itk_handler.GetSource(true); // active_slices -> correct seed z-position
		auto target = itk_handler.GetTarget(true);
		DoWorkNd<input_type>(source, target);
	}
	else
	{
		using input_type = itk::Image<float, 2>;
		auto source = itk_handler.GetSourceSlice();
		auto target = itk_handler.GetTargetSlice();
		DoWorkNd<input_type>(source, target);
	}
}

template<typename TInput>
void LevelsetWidget::DoWorkNd(TInput* input, TInput* target)
{
	itkStaticConstMacro(ImageDimension, size_t, TInput::ImageDimension);
	using input_type = TInput;
	using real_type = itk::Image<float, ImageDimension>;
	using mask_type = itk::Image<unsigned char, ImageDimension>;

	using fast_marching_type = itk::FastMarchingImageFilter<real_type, real_type>;
	using node_container_type = typename fast_marching_type::NodeContainer;
	using node_type = typename fast_marching_type::NodeType;

	// create filters
	auto fast_marching = fast_marching_type::New();
	auto threshold_levelset = itk::ThresholdSegmentationLevelSetImageFilter<real_type, input_type>::New();
	auto threshold = itk::BinaryThresholdImageFilter<real_type, mask_type>::New();

	// initialize levelset
	typename real_type::Pointer initial_levelset;
	if (m_InitFromTarget->isChecked())
	{
		// threshold target -> mask
		auto threshold_target = itk::BinaryThresholdImageFilter<input_type, real_type>::New();
		threshold_target->SetInput(target);
		threshold_target->SetLowerThreshold(0.001f);
		threshold_target->SetInsideValue(-0.5f); // level set filter uses iso-value 0.0
		threshold_target->SetOutsideValue(0.5f);
		threshold_target->Update();
		initial_levelset = threshold_target->GetOutput();
	}
	else
	{
		// setup seeds
		std::vector<typename input_type::IndexType> indices;
		GetSeeds(indices);

		const double initial_distance = 2.0;				 // \todo BL
		const double seed_value = -initial_distance; // \todo BL
		auto seeds = node_container_type::New();
		seeds->Initialize();
		for (size_t i = 0; i < indices.size(); ++i)
		{
			node_type node;
			node.SetValue(seed_value);
			node.SetIndex(indices[i]);
			seeds->InsertElement(i, node);
		}

		fast_marching->SetTrialPoints(seeds);
		fast_marching->SetSpeedConstant(1.0);
		fast_marching->SetOutputRegion(input->GetBufferedRegion());
		fast_marching->SetOutputSpacing(input->GetSpacing());
		fast_marching->SetOutputOrigin(input->GetOrigin());
		fast_marching->SetOutputDirection(input->GetDirection());
		initial_levelset = fast_marching->GetOutput();
	}

	// set parameters
	threshold_levelset->SetInput(initial_levelset);
	threshold_levelset->SetFeatureImage(input);
	threshold_levelset->SetPropagationScaling(1.0);
	threshold_levelset->SetCurvatureScaling(m_CurvatureScaling->text().toDouble());
	threshold_levelset->SetEdgeWeight(m_EdgeWeight->text().toDouble());
	threshold_levelset->SetLowerThreshold(m_LowerThreshold->text().toDouble());
	threshold_levelset->SetUpperThreshold(m_UpperThreshold->text().toDouble());
	threshold_levelset->SetMaximumRMSError(0.02);
	threshold_levelset->SetNumberOfIterations(1200);
	threshold_levelset->SetIsoSurfaceValue(0.0);

	threshold->SetInput(threshold_levelset->GetOutput());
	threshold->SetLowerThreshold(std::numeric_limits<float>::lowest());
	threshold->SetUpperThreshold(0);
	threshold->SetOutsideValue(0);
	threshold->SetInsideValue(255);

	try
	{
		threshold->Update();
		std::cerr << "Finished levelset segmentation\n";
	}
	catch (itk::ExceptionObject e)
	{
		ISEG_ERROR_MSG(e.what());
		return;
	}

	iseg::DataSelection data_selection;
	data_selection.allSlices = m_AllSlices->isChecked();
	data_selection.sliceNr = m_Activeslice;
	data_selection.work = true;
	emit BeginDatachange(data_selection, this);

	if (!iseg::Paste<mask_type, input_type>(threshold->GetOutput(), target))
	{
		ISEG_ERROR_MSG("could not set output because image regions don't match.");
	}

	emit EndDatachange(this);
}


//
// Copyright 2010 Scott McMurray.
// 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_HASH_DETAIL_EXPLODER_HPP
#define BOOST_HASH_DETAIL_EXPLODER_HPP

#include <boost/hash/stream_endian.hpp>
#include <boost/hash/detail/unbounded_shift.hpp>
#include <boost/integer.hpp>
#include <boost/static_assert.hpp>

#include <iterator>

#include <climits>
#include <cstring>

namespace boost {
namespace hashes {
namespace detail {

// By definition, for all exploders, InputBits > OutputBits,
// so we're taking one value and splitting it into many smaller values

template <typename OutIter, int OutBits,
          typename T = typename std::iterator_traits<OutIter>::value_type>
struct outvalue_helper {
    typedef T type;
};
template <typename OutIter, int OutBits>
struct outvalue_helper<OutIter, OutBits, void> {
    typedef typename uint_t<OutBits>::least type;
};

template <typename Endianness,
          int InputBits, int OutputBits,
          int k>
struct exploder_step;

template <int UnitBits, int InputBits, int OutputBits, int k>
struct exploder_step<stream_endian::big_unit_big_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutIter>
    static void step(InputValue const &x, OutIter &out) {
        int const shift = InputBits - (OutputBits+k);
        typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
        InputValue y = unbounded_shr<shift>(x);
        *out++ = OutValue(low_bits<OutputBits>(y));
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct exploder_step<stream_endian::little_unit_big_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutIter>
    static void step(InputValue const &x, OutIter &out) {
        int const kb = (k % UnitBits);
        int const ku = k - kb;
        int const shift =
            OutputBits >= UnitBits ? k :
            InputBits  >= UnitBits ? ku + (UnitBits-(OutputBits+kb)) :
                                     InputBits - (OutputBits+kb);
        typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
        InputValue y = unbounded_shr<shift>(x);
        *out++ = OutValue(low_bits<OutputBits>(y));
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct exploder_step<stream_endian::big_unit_little_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutIter>
    static void step(InputValue const &x, OutIter &out) {
        int const kb = (k % UnitBits);
        int const ku = k - kb;
        int const shift =
            OutputBits >= UnitBits ? InputBits - (OutputBits+k) :
            InputBits  >= UnitBits ? InputBits - (UnitBits+ku) + kb :
                                     kb;
        typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
        InputValue y = unbounded_shr<shift>(x);
        *out++ = OutValue(low_bits<OutputBits>(y));
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct exploder_step<stream_endian::little_unit_little_bit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutIter>
    static void step(InputValue const &x, OutIter &out) {
        int const shift = k;
        typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
        InputValue y = unbounded_shr<shift>(x);
        *out++ = OutValue(low_bits<OutputBits>(y));
    }
};

template <int UnitBits, int InputBits, int OutputBits, int k>
struct exploder_step<stream_endian::host_unit<UnitBits>,
                     InputBits, OutputBits, k> {
    template <typename InputValue, typename OutIter>
    static void step(InputValue const &x, OutIter &out) {
        typedef typename outvalue_helper<OutIter, OutputBits>::type OutValue;
        BOOST_STATIC_ASSERT(sizeof(InputValue)*CHAR_BIT == InputBits);
        BOOST_STATIC_ASSERT(sizeof(OutValue)*CHAR_BIT == OutputBits);
        OutValue value;
        std::memcpy(&value, (char*)&x + k/CHAR_BIT, OutputBits/CHAR_BIT);
        *out++ = value;
    }
};

template <typename Endianness,
          int InputBits, int OutputBits,
          int k = 0>
struct exploder;

template <template <int> class Endian, int UnitBits,
          int InputBits, int OutputBits,
          int k>
struct exploder<Endian<UnitBits>, InputBits, OutputBits, k> {

    // To keep the implementation managable, input and output sizes must
    // be multiples or factors of the unit size.
    // If one of these is firing, you may want a bit-only stream_endian
    // rather than one that mentions bytes or octets.
    BOOST_STATIC_ASSERT(!(InputBits  % UnitBits && UnitBits % InputBits ));
    BOOST_STATIC_ASSERT(!(OutputBits % UnitBits && UnitBits % OutputBits));

    typedef Endian<UnitBits> Endianness;
    typedef exploder_step<Endianness, InputBits, OutputBits, k> step_type;
    typedef exploder<Endianness, InputBits, OutputBits, k+OutputBits> next_type;

    template <typename InputValue, typename OutIter>
    static void explode(InputValue const &x, OutIter &out) {
        step_type::step(x, out);
        next_type::explode(x, out);
    }

};
template <template <int> class Endian, int UnitBits,
          int InputBits, int OutputBits>
struct exploder<Endian<UnitBits>, InputBits, OutputBits, InputBits> {
    template <typename InputValue, typename OutIter>
    static void explode(InputValue const &, OutIter &) {}
};

} // namespace detail
} // namespace hashes
} // namespace boost

#endif // BOOST_HASH_DETAIL_EXPLODER_HPP


/* Copyright (C) 2012-2020 IBM Corp.
 * This program is 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. See accompanying LICENSE file.
 */

/* Test_ThinEvalMap.cpp - Testing the evaluation map for thin bootstrapping
 */
#include <helib/helib.h>
#include <helib/EvalMap.h>
#include <NTL/BasicThreadPool.h>
#include <helib/debugging.h>

#include "gtest/gtest.h"
#include "test_common.h"

namespace {

struct Parameters
{

  const long p;
  const long r;
  const long c;
  const long k;
  const long L;
  const long s;
  const long seed;
  const NTL::Vec<long> mvec;
  const std::vector<long> gens;
  const std::vector<long> ords;
  const long nthreads;
  const long useCache;

  Parameters(long p,
             long r,
             long c,
             long k,
             long L,
             long s,
             long seed,
             NTL::Vec<long> mvec,
             std::vector<long> gens,
             std::vector<long> ords,
             long nthreads,
             long useCache) :
      p(p),
      r(r),
      c(c),
      k(k),
      L(L),
      s(s),
      seed(seed),
      mvec(mvec),
      gens(gens),
      ords(ords),
      nthreads(nthreads),
      useCache(useCache){};

  friend std::ostream& operator<<(std::ostream& os, const Parameters& params)
  {
    return os << "{"
              << "p=" << params.p << ","
              << "r=" << params.r << ","
              << "c=" << params.c << ","
              << "k=" << params.k << ","
              << "L=" << params.L << ","
              << "s=" << params.s << ","
              << "seed=" << params.seed << ","
              << "mvec=" << params.mvec << ","
              << "gens=" << helib::vecToStr(params.gens) << ","
              << "ords=" << helib::vecToStr(params.ords) << ","
              << "nthreads=" << params.nthreads << ","
              << "useCache=" << params.useCache << "}";
  };
};

class GTestThinEvalMap : public ::testing::TestWithParam<Parameters>
{
protected:
  const long p;
  const long r;
  const long c;
  const long k;
  const long w;
  const long L;
  const long s;
  const long seed;
  const NTL::Vec<long> mvec;
  const std::vector<long> gens;
  const std::vector<long> ords;
  const long m;
  const long nthreads;
  const long useCache;
  helib::Context context;
  const long d;
  const long phim;
  const long nslots;
  helib::SecKey secretKey;
  const helib::PubKey& publicKey;

  static std::vector<long> getDefaultGens()
  {
    return std::vector<long>{3979, 3095, 3760};
  };

  static std::vector<long> getDefaultOrds()
  {
    return std::vector<long>{6, 2, -8};
  };

  static NTL::Vec<long> getDefaultMvec()
  {
    NTL::Vec<long> defaultMvec;
    defaultMvec.SetLength(3);
    defaultMvec[0] = 7;
    defaultMvec[1] = 3;
    defaultMvec[2] = 221;
    return defaultMvec;
  };

  static void validateMvec(const NTL::Vec<long>& mvec)
  {
    long nfactors = mvec.length();
    for (long i = 0; i < nfactors; i++)
      for (long j = i + 1; j < nfactors; j++)
        if (NTL::GCD(mvec[i], mvec[j]) != 1)
          throw std::invalid_argument(
              "mvec must include the prime-power factorization of m");
  };

  static NTL::Vec<long> calculateMvec(const NTL::Vec<long>& userMvec)
  {
    NTL::Vec<long> mvec;
    if (helib::lsize(userMvec) >= 1)
      mvec = userMvec;
    else
      mvec = getDefaultMvec();
    validateMvec(mvec);
    return mvec;
  };

  static helib::Context& prepareContext(helib::Context& context,
                                        const long L,
                                        const long c)
  {
    helib::buildModChain(context, L, c);
    if (!helib_test::noPrint) {
      context.zMStar.printout(); // print structure of Zm* /(p) to std::cout
      std::cout << std::endl;
    }
    return context;
  };

  // FIXME: Due to Victor removing skhwt this needs looking at.
  static helib::SecKey& prepareSecretKey(helib::SecKey& secretKey,
                                         UNUSED long w)
  {
    secretKey.GenSecKey();
    helib::addSome1DMatrices(
        secretKey); // compute key-switching matrices that we need
    helib::addFrbMatrices(
        secretKey); // compute key-switching matrices that we need
    return secretKey;
  }

  GTestThinEvalMap() :
      p(GetParam().p),
      r(GetParam().r),
      c(GetParam().c),
      k(GetParam().k),
      w(64),
      L(GetParam().L),
      s(GetParam().s),
      seed(
          (NTL::SetSeed(NTL::conv<NTL::ZZ>(GetParam().seed)), GetParam().seed)),
      mvec(calculateMvec(GetParam().mvec)),
      gens(helib::lsize(GetParam().mvec) >= 1 ? GetParam().gens
                                              : getDefaultGens()),
      ords(helib::lsize(GetParam().mvec) >= 1 ? GetParam().ords
                                              : getDefaultOrds()),
      m(helib::computeProd(mvec)),
      nthreads((NTL::SetNumThreads(GetParam().nthreads), GetParam().nthreads)),
      useCache(GetParam().useCache),
      context((helib::setTimersOn(),
               helib::setDryRun(
                   false), // Need to get a "real context" to test ThinEvalMap
               m),
              p,
              r,
              gens,
              ords),
      d(prepareContext(context, L, c).zMStar.getOrdP()),
      phim(context.zMStar.getPhiM()),
      nslots(phim / d),
      secretKey(
          (helib::setDryRun(
               helib_test::dry), // Now we can set the dry-run flag if desired
           context)),
      publicKey(prepareSecretKey(secretKey, w)){};

  virtual void TearDown() override
  {
    if (!helib_test::noPrint) {
      std::cout << "\n*********\n";
      helib::printAllTimers();
      std::cout << std::endl;
    }
    helib::cleanupDebugGlobals();
  }
};

TEST_P(GTestThinEvalMap, thinEvalMapIsCorrect)
{
  // GG defines the plaintext space Z_p[X]/GG(X)
  NTL::ZZX GG;
  GG = context.alMod.getFactorsOverZZ()[0];
  helib::EncryptedArray ea(context, GG);

  NTL::zz_p::init(context.alMod.getPPowR());

  NTL::Vec<NTL::zz_p> val0(NTL::INIT_SIZE, nslots);
  for (auto& x : val0)
    random(x);

  std::vector<NTL::ZZX> val1;
  val1.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    val1[i] = NTL::conv<NTL::ZZX>(NTL::conv<NTL::ZZ>(rep(val0[i])));
  }

  helib::Ctxt ctxt(publicKey);
  ea.encrypt(ctxt, publicKey, val1);

  helib::resetAllTimers();
  HELIB_NTIMER_START(ALL);

  // Compute homomorphically the transformation that takes the
  // coefficients packed in the slots and produces the polynomial
  // corresponding to cube

  if (!helib_test::noPrint)
    helib::CheckCtxt(ctxt, "init");

  if (!helib_test::noPrint)
    std::cout << "build ThinEvalMap\n";

  helib::ThinEvalMap map(ea,
                         /*minimal=*/false,
                         mvec,
                         /*invert=*/false,
                         /*build_cache=*/false);
  // compute the transformation to apply

  if (!helib_test::noPrint)
    std::cout << "apply ThinEvalMap\n";
  if (useCache)
    map.upgrade();
  map.apply(ctxt); // apply the transformation to ctxt
  if (!helib_test::noPrint)
    helib::CheckCtxt(ctxt, "ThinEvalMap");
  if (!helib_test::noPrint)
    std::cout << "check results\n";

  if (!helib_test::noPrint)
    std::cout << "build ThinEvalMap\n";
  helib::ThinEvalMap imap(ea,
                          /*minimal=*/false,
                          mvec,
                          /*invert=*/true,
                          /*build_cache=*/false);
  // compute the transformation to apply
  if (!helib_test::noPrint)
    std::cout << "apply ThinEvalMap\n";
  if (useCache)
    imap.upgrade();
  imap.apply(ctxt); // apply the transformation to ctxt
  if (!helib_test::noPrint) {
    helib::CheckCtxt(ctxt, "ThinEvalMap");
    std::cout << "check results\n";
  }

#if 1

  /* create dirty version of ctxt */
  NTL::Vec<NTL::zz_pX> dirty_val0;
  dirty_val0.SetLength(nslots);
  for (long i = 0; i < nslots; i++) {
    random(dirty_val0[i], d);
    SetCoeff(dirty_val0[i], 0, val0[i]);
  }

  std::vector<NTL::ZZX> dirty_val1;
  dirty_val1.resize(nslots);
  for (long i = 0; i < nslots; i++) {
    dirty_val1[i] = NTL::conv<NTL::ZZX>(dirty_val0[i]);
  }

  helib::Ctxt dirty_ctxt(publicKey);
  ea.encrypt(dirty_ctxt, publicKey, dirty_val1);

  helib::EvalMap dirty_map(ea,
                           /*minimal=*/false,
                           mvec,
                           /*invert=*/false,
                           /*build_cache=*/false);

  dirty_map.apply(dirty_ctxt);
  imap.apply(dirty_ctxt);
#endif

  std::vector<NTL::ZZX> val2;
  ea.decrypt(ctxt, secretKey, val2);

  EXPECT_EQ(val1, val2);

#if 1
  std::vector<NTL::ZZX> dirty_val2;
  ea.decrypt(dirty_ctxt, secretKey, dirty_val2);

  EXPECT_EQ(val1, dirty_val2);
#endif

  HELIB_NTIMER_STOP(ALL);
}

// clang-format off
INSTANTIATE_TEST_SUITE_P(variousParameters, GTestThinEvalMap, ::testing::Values(
    //SLOW
    Parameters(2, 1, 2, 80, 300, 0, 0,
               helib::convert<NTL::Vec<long>, std::vector<long>>(std::vector<long>{7, 3, 221}),
               std::vector<long>{3979, 3095, 3760},
               std::vector<long>{6, 2, -8},
               1, 0)
    //FAST
    //Parameters(2, 1, 2, 80, 300, 0, 0,
    //           helib::convert<NTL::Vec<long>, std::vector<long>>(std::vector<long>{3, 35}),
    //           std::vector<long>{71, 76},
    //           std::vector<long>{2, 2},
    //           1, 0)
    ));
// clang-format on
} // namespace


/*
 boost header: numeric/odeint/gram_schmitt.hpp

 Copyright 2011-2013 Karsten Ahnert
 Copyright 2011 Mario Mulansky

 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_NUMERIC_ODEINT_GRAM_SCHMITT_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_GRAM_SCHMITT_HPP_INCLUDED

#include <boost/throw_exception.hpp>
#include <iterator>
#include <algorithm>
#include <numeric>

namespace boost {
namespace numeric {
namespace odeint {

template< class Iterator , class T >
void normalize( Iterator first , Iterator last , T norm )
{
    while( first != last ) *first++ /= norm;
}

template< class Iterator , class T >
void substract_vector( Iterator first1 , Iterator last1 ,
        Iterator first2 , T val )
{
    while( first1 != last1 ) *first1++ -= val * ( *first2++ );
}

template< size_t num_of_lyap , class StateType , class LyapType >
void gram_schmidt( StateType &x , LyapType &lyap , size_t n )
{
    if( !num_of_lyap ) return;
    if( ptrdiff_t( ( num_of_lyap + 1 ) * n ) != std::distance( x.begin() , x.end() ) )
        BOOST_THROW_EXCEPTION( std::domain_error( "renormalization() : size of state does not match the number of lyapunov exponents." ) );

    typedef typename StateType::value_type value_type;
    typedef typename StateType::iterator iterator;

    value_type norm[num_of_lyap];
    value_type tmp[num_of_lyap];
    iterator first = x.begin() + n;
    iterator beg1 = first , end1 = first + n ;

    std::fill( norm , norm+num_of_lyap , 0.0 );

    // normalize first vector
    norm[0] = sqrt( std::inner_product( beg1 , end1 , beg1 , 0.0 ) );
    normalize( beg1 , end1 , norm[0] );

    beg1 += n;
    end1 += n;

    for( size_t j=1 ; j<num_of_lyap ; ++j , beg1+=n , end1+=n )
    {
        for( size_t k=0 ; k<j ; ++k )
        {
            tmp[k] = std::inner_product( beg1 , end1 , first + k*n , 0.0 );
            //  clog << j << " " << k << " " << tmp[k] << "\n";
        }

        for( size_t k=0 ; k<j ; ++k )
            substract_vector( beg1 , end1 , first + k*n , tmp[k] );

        // normalize j-th vector
        norm[j] = sqrt( std::inner_product( beg1 , end1 , beg1 , 0.0 ) );
        // clog << j << " " << norm[j] << "\n";
        normalize( beg1 , end1 , norm[j] );
    }

    for( size_t j=0 ; j<num_of_lyap ; j++ )
        lyap[j] += log( norm[j] );
}

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif //BOOST_NUMERIC_ODEINT_GRAM_SCHMITT_HPP_INCLUDED


#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 <limits>
#include <vector>

class FrechetTestRig : public VectorRealRNGTestRig {
 public:
  FrechetTestRig()
      : 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}) {}

  /*
   * Since boost does not have a Frechet distribution, generate values from
   * a Weibull distribution using the Frechet rng, and compare against the
   * quantiles from a Weibull distribution.
   */
  template <typename T1, typename T2, typename T3, typename T_rng>
  auto generate_samples(const T1& alpha, const T2& sigma, const T3&,
                        T_rng& rng) const {
    return stan::math::inv(stan::math::frechet_rng(alpha, sigma, rng));
  }

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

    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(ProbDistributionsFrechet, errorCheck) {
  check_dist_throws_all_types(FrechetTestRig());
}

TEST(ProbDistributionsFrechet, distributionTest) {
  check_quantiles_real_real(FrechetTestRig());
}

TEST(ProbDistributionsFrechet, error_check) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::math::frechet_rng(2.0, 3.0, rng));

  EXPECT_THROW(stan::math::frechet_rng(-2.0, 3.0, rng), std::domain_error);
  EXPECT_THROW(stan::math::frechet_rng(2.0, -3.0, rng), std::domain_error);
  EXPECT_THROW(
      stan::math::frechet_rng(stan::math::positive_infinity(), 3.0, rng),
      std::domain_error);
}

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

  // and invert them to compare to Weibull
  std::vector<double> samples;
  for (int i = 0; i < N; ++i) {
    samples.push_back(1.0 / stan::math::frechet_rng(2.0, 1.0 / 5.0, rng));
  }

  // Generate quantiles from boost's Frechet distribution
  // Actually, boost doesn't have a Frechet distribution
  // So we use the Weibull.
  boost::math::weibull_distribution<> dist(2.0, 5.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 (c) 2017 CNRS
//
// This file is part of tsid
// tsid is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
// tsid is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// tsid If not, see
// <http://www.gnu.org/licenses/>.
//

#include <iostream>

#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>

#include <tsid/math/constraint-bound.hpp>
#include <tsid/math/constraint-equality.hpp>
#include <tsid/math/constraint-inequality.hpp>

BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )

BOOST_AUTO_TEST_CASE ( test_constraint_bounds )
{
  std::cout << "test_constraint_bounds\n";
  using namespace tsid::math;
  using namespace Eigen;
  const unsigned int n = 5;

  VectorXd lb = -1.0*VectorXd::Ones(n);
  VectorXd ub = VectorXd::Ones(n);
  ConstraintBound bounds("bounds", lb, ub);

  BOOST_CHECK(bounds.isBound());
  BOOST_CHECK(!bounds.isEquality());
  BOOST_CHECK(!bounds.isInequality());

  BOOST_CHECK(bounds.rows()==n);
  BOOST_CHECK(bounds.cols()==n);

  BOOST_CHECK(lb.isApprox(bounds.lowerBound()));
  BOOST_CHECK(ub.isApprox(bounds.upperBound()));

  lb *= 2.0;
  BOOST_CHECK(! lb.isApprox(bounds.lowerBound()));
  bounds.setLowerBound(lb);
  BOOST_CHECK(lb.isApprox(bounds.lowerBound()));

  ub *= 2.0;
  BOOST_CHECK(! ub.isApprox(bounds.upperBound()));
  bounds.setUpperBound(ub);
  BOOST_CHECK(ub.isApprox(bounds.upperBound()));
}

BOOST_AUTO_TEST_CASE ( test_constraint_equality )
{
  using namespace tsid::math;
  using namespace Eigen;
  using namespace std;

  const unsigned int n = 5;
  const unsigned int m = 2;

  MatrixXd A = MatrixXd::Ones(m,n);
  VectorXd b = VectorXd::Ones(m);
  ConstraintEquality equality("equality", A, b);

  BOOST_CHECK(!equality.isBound());
  BOOST_CHECK(equality.isEquality());
  BOOST_CHECK(!equality.isInequality());

  BOOST_CHECK(equality.rows()==m);
  BOOST_CHECK(equality.cols()==n);

  BOOST_CHECK(A.isApprox(equality.matrix()));
  BOOST_CHECK(b.isApprox(equality.vector()));

  b *= 2.0;
  BOOST_CHECK(! b.isApprox(equality.vector()));
  equality.setVector(b);
  BOOST_CHECK(b.isApprox(equality.vector()));

  A *= 2.0;
  BOOST_CHECK(! A.isApprox(equality.matrix()));
  equality.setMatrix(A);
  BOOST_CHECK(A.isApprox(equality.matrix()));
}

BOOST_AUTO_TEST_CASE ( test_constraint_inequality )
{
  using namespace tsid::math;
  using namespace Eigen;
  using namespace std;

  const unsigned int n = 5;
  const unsigned int m = 2;

  MatrixXd A = MatrixXd::Ones(m,n);
  VectorXd lb = -1.0*VectorXd::Ones(m);
  VectorXd ub = VectorXd::Ones(m);
  ConstraintInequality inequality("inequality", A, lb, ub);

  BOOST_CHECK(!inequality.isBound());
  BOOST_CHECK(!inequality.isEquality());
  BOOST_CHECK(inequality.isInequality());

  BOOST_CHECK(inequality.rows()==m);
  BOOST_CHECK(inequality.cols()==n);

  BOOST_CHECK(A.isApprox(inequality.matrix()));
  BOOST_CHECK(lb.isApprox(inequality.lowerBound()));
  BOOST_CHECK(ub.isApprox(inequality.upperBound()));

  lb *= 2.0;
  BOOST_CHECK(! lb.isApprox(inequality.lowerBound()));
  inequality.setLowerBound(lb);
  BOOST_CHECK(lb.isApprox(inequality.lowerBound()));

  A *= 2.0;
  BOOST_CHECK(! A.isApprox(inequality.matrix()));
  inequality.setMatrix(A);
  BOOST_CHECK(A.isApprox(inequality.matrix()));
}

BOOST_AUTO_TEST_SUITE_END ()


#ifndef HYPSYS1D_CFL_CONDITION_HPP
#define HYPSYS1D_CFL_CONDITION_HPP

#include <cmath>
#include <limits>
#include <memory>

#include <Eigen/Dense>

#include <ancse/includes.hpp>
#include <ancse/grid.hpp>
#include <ancse/model.hpp>
#include <ancse/dg_handler.hpp>

/// Interface for computing CFL restricted timesteps.
class CFLCondition {
  public:
    virtual ~CFLCondition() = default;

    /// Compute the largest time-step satisfying the CFL condition.
    virtual double operator()(const Eigen::MatrixXd &u) const = 0;
};

class FVMCFLCondition : public CFLCondition
{
    public:
        FVMCFLCondition(const Grid& grid,
                        const std::shared_ptr<Model>& model,
                        double cfl_number);

        virtual double operator() (const Eigen::MatrixXd& u) const override;

    private:
        Grid grid;
        std::shared_ptr<Model> model;
        double cfl_number;
};

class DGCFLCondition: public CFLCondition
{
    public:
        DGCFLCondition(const Grid& grid,
                        const std::shared_ptr<Model>& model,
                        const DGHandler& dg_handler,
                        double cfl_number);

        virtual double operator() (const Eigen::MatrixXd& u) const override;

    private:
        Grid grid;
        std::shared_ptr<Model> model;
        DGHandler dg_handler;
        double cfl_number;
};

/// make CFL condition for FVM
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid,
                   const std::shared_ptr<Model> &model,
                   double cfl_number);

/// make CFL condition for DG
std::shared_ptr<CFLCondition>
make_cfl_condition(const Grid &grid,
                   const std::shared_ptr<Model> &model,
                   const DGHandler &dg_handler,
                   double cfl_number);

#endif // HYPSYS1D_CFL_CONDITION_HPP


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <boost/simd/arithmetic/include/functions/simd/fast_rsqrt.hpp>
#include <boost/simd/include/functions/splat.hpp>
#include <boost/dispatch/functor/meta/call.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>
#include <boost/simd/include/constants/four.hpp>
#include <boost/simd/include/constants/half.hpp>
#include <boost/simd/include/constants/sqrt_2.hpp>

#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <nt2/sdk/unit/module.hpp>

NT2_TEST_CASE_TPL( fast_rsqrt, BOOST_SIMD_SIMD_REAL_TYPES )
{
  using boost::simd::fast_rsqrt;
  using boost::simd::tag::fast_rsqrt_;
  using boost::simd::native;
  using boost::simd::splat;

  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;

  // return type conformity test
  NT2_TEST_TYPE_IS( typename boost::dispatch::meta::call<fast_rsqrt_(vT)>::type
                  , vT
                  );

  // specific values tests
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::Mone<vT>()), boost::simd::Nan<vT>(), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::Nan<vT>()), boost::simd::Nan<vT>(), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::One<vT>()), boost::simd::One<vT>(), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(boost::simd::Four<vT>()), boost::simd::Half<vT>(), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(splat<vT>(0.5)), boost::simd::Sqrt_2<vT>(), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(splat<vT>(0.01)), splat<vT>(10), 75);
  NT2_TEST_ULP_EQUAL(fast_rsqrt(splat<vT>(0.0001)), splat<vT>(100), 75);
}