// Boost.Geometry Index
//
// R-tree R*-tree split algorithm implementation
//
// Copyright (c) 2011-2014 Adam Wulkiewicz, Lodz, Poland.
//
// 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)

#ifndef BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP
#define BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP

#include <boost/geometry/index/detail/algorithms/intersection_content.hpp>
#include <boost/geometry/index/detail/algorithms/union_content.hpp>
#include <boost/geometry/index/detail/algorithms/margin.hpp>

#include <boost/geometry/index/detail/bounded_view.hpp>

#include <boost/geometry/index/detail/rtree/node/node.hpp>
#include <boost/geometry/index/detail/rtree/visitors/insert.hpp>
#include <boost/geometry/index/detail/rtree/visitors/is_leaf.hpp>

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry { namespace index {

namespace detail { namespace rtree {

namespace rstar {

template <typename Element, typename Translator, typename Tag, size_t Corner, size_t AxisIndex>
class element_axis_corner_less
{
    typedef typename rtree::element_indexable_type<Element, Translator>::type indexable_type;
    typedef typename geometry::point_type<indexable_type>::type point_type;
    typedef geometry::model::box<point_type> bounds_type;
    typedef index::detail::bounded_view<indexable_type, bounds_type> bounded_view_type;

public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        bounded_view_type bounded_ind1(rtree::element_indexable(e1, m_tr));
        bounded_view_type bounded_ind2(rtree::element_indexable(e2, m_tr));

        return geometry::get<Corner, AxisIndex>(bounded_ind1)
            < geometry::get<Corner, AxisIndex>(bounded_ind2);
    }

private:
    Translator const& m_tr;
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, box_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<Corner, AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<Corner, AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Element, typename Translator, size_t Corner, size_t AxisIndex>
class element_axis_corner_less<Element, Translator, point_tag, Corner, AxisIndex>
{
public:
    element_axis_corner_less(Translator const& tr)
        : m_tr(tr)
    {}

    bool operator()(Element const& e1, Element const& e2) const
    {
        return geometry::get<AxisIndex>(rtree::element_indexable(e1, m_tr))
            < geometry::get<AxisIndex>(rtree::element_indexable(e2, m_tr));
    }

private:
    Translator const& m_tr;
};

template <typename Box, size_t Corner, size_t AxisIndex>
struct choose_split_axis_and_index_for_corner
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename Elements::value_type element_type;
        typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
        typedef typename tag<indexable_type>::type indexable_tag;

        BOOST_GEOMETRY_INDEX_ASSERT(elements.size() == parameters.get_max_elements() + 1, "wrong number of elements");

        // copy elements
        Elements elements_copy(elements);                                                                       // MAY THROW, STRONG (alloc, copy)

        size_t const index_first = parameters.get_min_elements();
        size_t const index_last = parameters.get_max_elements() - parameters.get_min_elements() + 2;

        // sort elements
        element_axis_corner_less<element_type, Translator, indexable_tag, Corner, AxisIndex> elements_less(translator);
        std::sort(elements_copy.begin(), elements_copy.end(), elements_less);                                   // MAY THROW, BASIC (copy)
//        {
//            typename Elements::iterator f = elements_copy.begin() + index_first;
//            typename Elements::iterator l = elements_copy.begin() + index_last;
//            std::nth_element(elements_copy.begin(), f, elements_copy.end(), elements_less);                   // MAY THROW, BASIC (copy)
//            std::nth_element(f, l, elements_copy.end(), elements_less);                                       // MAY THROW, BASIC (copy)
//            std::sort(f, l, elements_less);                                                                   // MAY THROW, BASIC (copy)
//        }

        // init outputs
        choosen_index = index_first;
        sum_of_margins = 0;
        smallest_overlap = (std::numeric_limits<content_type>::max)();
        smallest_content = (std::numeric_limits<content_type>::max)();

        // calculate sum of margins for all distributions
        for ( size_t i = index_first ; i < index_last ; ++i )
        {
            // TODO - awulkiew: may be optimized - box of group 1 may be initialized with
            // box of min_elems number of elements and expanded for each iteration by another element

            Box box1 = rtree::elements_box<Box>(elements_copy.begin(), elements_copy.begin() + i, translator);
            Box box2 = rtree::elements_box<Box>(elements_copy.begin() + i, elements_copy.end(), translator);

            sum_of_margins += index::detail::comparable_margin(box1) + index::detail::comparable_margin(box2);

            content_type ovl = index::detail::intersection_content(box1, box2);
            content_type con = index::detail::content(box1) + index::detail::content(box2);

            // TODO - shouldn't here be < instead of <= ?
            if ( ovl < smallest_overlap || (ovl == smallest_overlap && con <= smallest_content) )
            {
                choosen_index = i;
                smallest_overlap = ovl;
                smallest_content = con;
            }
        }

        ::geofeatures_boost::ignore_unused_variable_warning(parameters);
    }
};

//template <typename Box, size_t AxisIndex, typename ElementIndexableTag>
//struct choose_split_axis_and_index_for_axis
//{
//    BOOST_MPL_ASSERT_MSG(false, NOT_IMPLEMENTED_FOR_THIS_TAG, (ElementIndexableTag));
//};

template <typename Box, size_t AxisIndex, typename ElementIndexableTag>
struct choose_split_axis_and_index_for_axis
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        size_t index1 = 0;
        margin_type som1 = 0;
        content_type ovl1 = (std::numeric_limits<content_type>::max)();
        content_type con1 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Box, min_corner, AxisIndex>
            ::apply(elements, index1,
                    som1, ovl1, con1,
                    parameters, translator);                                                                // MAY THROW, STRONG

        size_t index2 = 0;
        margin_type som2 = 0;
        content_type ovl2 = (std::numeric_limits<content_type>::max)();
        content_type con2 = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_corner<Box, max_corner, AxisIndex>
            ::apply(elements, index2,
                    som2, ovl2, con2,
                    parameters, translator);                                                                // MAY THROW, STRONG

        sum_of_margins = som1 + som2;

        if ( ovl1 < ovl2 || (ovl1 == ovl2 && con1 <= con2) )
        {
            choosen_corner = min_corner;
            choosen_index = index1;
            smallest_overlap = ovl1;
            smallest_content = con1;
        }
        else
        {
            choosen_corner = max_corner;
            choosen_index = index2;
            smallest_overlap = ovl2;
            smallest_content = con2;
        }
    }
};

template <typename Box, size_t AxisIndex>
struct choose_split_axis_and_index_for_axis<Box, AxisIndex, point_tag>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        choose_split_axis_and_index_for_corner<Box, min_corner, AxisIndex>
            ::apply(elements, choosen_index,
                    sum_of_margins, smallest_overlap, smallest_content,
                    parameters, translator);                                                                // MAY THROW, STRONG

        choosen_corner = min_corner;
    }
};

template <typename Box, size_t Dimension>
struct choose_split_axis_and_index
{
    BOOST_STATIC_ASSERT(0 < Dimension);

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choose_split_axis_and_index<Box, Dimension - 1>
            ::apply(elements, choosen_axis, choosen_corner, choosen_index,
                    smallest_sum_of_margins, smallest_overlap, smallest_content,
                    parameters, translator);                                                                // MAY THROW, STRONG

        margin_type sum_of_margins = 0;

        size_t corner = min_corner;
        size_t index = 0;

        content_type overlap_val = (std::numeric_limits<content_type>::max)();
        content_type content_val = (std::numeric_limits<content_type>::max)();

        choose_split_axis_and_index_for_axis<
            Box,
            Dimension - 1,
            typename tag<element_indexable_type>::type
        >::apply(elements, corner, index, sum_of_margins, overlap_val, content_val, parameters, translator); // MAY THROW, STRONG

        if ( sum_of_margins < smallest_sum_of_margins )
        {
            choosen_axis = Dimension - 1;
            choosen_corner = corner;
            choosen_index = index;
            smallest_sum_of_margins = sum_of_margins;
            smallest_overlap = overlap_val;
            smallest_content = content_val;
        }
    }
};

template <typename Box>
struct choose_split_axis_and_index<Box, 1>
{
    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Elements, typename Parameters, typename Translator>
    static inline void apply(Elements const& elements,
                             size_t & choosen_axis,
                             size_t & choosen_corner,
                             size_t & choosen_index,
                             margin_type & smallest_sum_of_margins,
                             content_type & smallest_overlap,
                             content_type & smallest_content,
                             Parameters const& parameters,
                             Translator const& translator)
    {
        typedef typename rtree::element_indexable_type<typename Elements::value_type, Translator>::type element_indexable_type;

        choosen_axis = 0;

        choose_split_axis_and_index_for_axis<
            Box,
            0,
            typename tag<element_indexable_type>::type
        >::apply(elements, choosen_corner, choosen_index, smallest_sum_of_margins, smallest_overlap, smallest_content, parameters, translator); // MAY THROW
    }
};

template <size_t Corner, size_t Dimension, size_t I = 0>
struct nth_element
{
    BOOST_STATIC_ASSERT(0 < Dimension);
    BOOST_STATIC_ASSERT(I < Dimension);

    template <typename Elements, typename Translator>
    static inline void apply(Elements & elements, const size_t axis, const size_t index, Translator const& tr)
    {
        //BOOST_GEOMETRY_INDEX_ASSERT(axis < Dimension, "unexpected axis value");

        if ( axis != I )
        {
            nth_element<Corner, Dimension, I + 1>::apply(elements, axis, index, tr);                          // MAY THROW, BASIC (copy)
        }
        else
        {
            typedef typename Elements::value_type element_type;
            typedef typename rtree::element_indexable_type<element_type, Translator>::type indexable_type;
            typedef typename tag<indexable_type>::type indexable_tag;

            element_axis_corner_less<element_type, Translator, indexable_tag, Corner, I> less(tr);
            std::nth_element(elements.begin(), elements.begin() + index, elements.end(), less);            // MAY THROW, BASIC (copy)
        }
    }
};

template <size_t Corner, size_t Dimension>
struct nth_element<Corner, Dimension, Dimension>
{
    template <typename Elements, typename Translator>
    static inline void apply(Elements & /*elements*/, const size_t /*axis*/, const size_t /*index*/, Translator const& /*tr*/)
    {}
};

} // namespace rstar

template <typename Value, typename Options, typename Translator, typename Box, typename Allocators>
struct redistribute_elements<Value, Options, Translator, Box, Allocators, rstar_tag>
{
    typedef typename rtree::node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type node;
    typedef typename rtree::internal_node<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type internal_node;
    typedef typename rtree::leaf<Value, typename Options::parameters_type, Box, Allocators, typename Options::node_tag>::type leaf;

    typedef typename Options::parameters_type parameters_type;

    static const size_t dimension = geometry::dimension<Box>::value;

    typedef typename index::detail::default_margin_result<Box>::type margin_type;
    typedef typename index::detail::default_content_result<Box>::type content_type;

    template <typename Node>
    static inline void apply(
        Node & n,
        Node & second_node,
        Box & box1,
        Box & box2,
        parameters_type const& parameters,
        Translator const& translator,
        Allocators & allocators)
    {
        typedef typename rtree::elements_type<Node>::type elements_type;
        typedef typename elements_type::value_type element_type;

        elements_type & elements1 = rtree::elements(n);
        elements_type & elements2 = rtree::elements(second_node);

        // copy original elements - use in-memory storage (std::allocator)
        // TODO: move if noexcept
        typedef typename rtree::container_from_elements_type<elements_type, element_type>::type
            container_type;
        container_type elements_copy(elements1.begin(), elements1.end());                               // MAY THROW, STRONG
        container_type elements_backup(elements1.begin(), elements1.end());                             // MAY THROW, STRONG

        size_t split_axis = 0;
        size_t split_corner = 0;
        size_t split_index = parameters.get_min_elements();
        margin_type smallest_sum_of_margins = (std::numeric_limits<margin_type>::max)();
        content_type smallest_overlap = (std::numeric_limits<content_type>::max)();
        content_type smallest_content = (std::numeric_limits<content_type>::max)();

        // NOTE: this function internally copies passed elements
        //       why not pass mutable elements and use the same container for all axes/corners
        //       and again, the same below calling partial_sort/nth_element
        //       It would be even possible to not re-sort/find nth_element if the axis/corner
        //       was found for the last sorting - last combination of axis/corner
        rstar::choose_split_axis_and_index<Box, dimension>
            ::apply(elements_copy,
                    split_axis, split_corner, split_index,
                    smallest_sum_of_margins, smallest_overlap, smallest_content,
                    parameters, translator);                                                            // MAY THROW, STRONG

        // TODO: awulkiew - get rid of following static_casts?
        BOOST_GEOMETRY_INDEX_ASSERT(split_axis < dimension, "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(split_corner == static_cast<size_t>(min_corner) || split_corner == static_cast<size_t>(max_corner), "unexpected value");
        BOOST_GEOMETRY_INDEX_ASSERT(parameters.get_min_elements() <= split_index && split_index <= parameters.get_max_elements() - parameters.get_min_elements() + 1, "unexpected value");

        // TODO: consider using nth_element
        if ( split_corner == static_cast<size_t>(min_corner) )
        {
            rstar::nth_element<min_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)
        }
        else
        {
            rstar::nth_element<max_corner, dimension>
                ::apply(elements_copy, split_axis, split_index, translator);                            // MAY THROW, BASIC (copy)
        }

        BOOST_TRY
        {
            // copy elements to nodes
            elements1.assign(elements_copy.begin(), elements_copy.begin() + split_index);               // MAY THROW, BASIC
            elements2.assign(elements_copy.begin() + split_index, elements_copy.end());                 // MAY THROW, BASIC

            // calculate boxes
            box1 = rtree::elements_box<Box>(elements1.begin(), elements1.end(), translator);
            box2 = rtree::elements_box<Box>(elements2.begin(), elements2.end(), translator);
        }
        BOOST_CATCH(...)
        {
            //elements_copy.clear();
            elements1.clear();
            elements2.clear();

            rtree::destroy_elements<Value, Options, Translator, Box, Allocators>::apply(elements_backup, allocators);
            //elements_backup.clear();

            BOOST_RETHROW                                                                                // RETHROW, BASIC
        }
        BOOST_CATCH_END
    }
};

}} // namespace detail::rtree

}}} // namespace geofeatures_boost::geometry::index

#endif // BOOST_GEOMETRY_INDEX_DETAIL_RTREE_RSTAR_REDISTRIBUTE_ELEMENTS_HPP


//==================================================================================================
/**
  Copyright 2017 NumScale SAS

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

#ifndef BOOST_SIMD_FUNCTION_SCALAR_SQR_ABS_HPP_INCLUDED
#define BOOST_SIMD_FUNCTION_SCALAR_SQR_ABS_HPP_INCLUDED

#include <boost/simd/function/definition/sqr_abs.hpp>
#include <boost/simd/arch/common/scalar/function/sqr_abs.hpp>

#endif


/**
 * Copyright Soramitsu Co., Ltd. All Rights Reserved.
 * SPDX-License-Identifier: Apache-2.0
 */

#include "interfaces/common_objects/amount.hpp"

#include <sstream>
#include <type_traits>
#include <utility>

#include <gtest/gtest.h>
#include <boost/math/special_functions/sign.hpp>

using namespace shared_model::interface;

struct AmountTest : public ::testing::Test {
  /// Check sign, precision and string representation of a valid amount.
  void checkValid(const Amount &tested,
                  int ref_sign,
                  types::PrecisionType ref_precision,
                  std::string ref_str) {
    EXPECT_EQ(boost::math::sign(tested.sign()), boost::math::sign(ref_sign));
    EXPECT_EQ(tested.precision(), ref_precision);
    EXPECT_EQ(tested.toStringRepr(), ref_str);
  }

  /// Check sign, precision and string representation of an invalid amount.
  void checkInvalid(const Amount &tested) {
    EXPECT_EQ(tested.sign(), 0);
    EXPECT_EQ(tested.precision(), 0);
    EXPECT_EQ(tested.toStringRepr(), "NaN");
  }
};

TEST_F(AmountTest, Basic) {
  checkValid(Amount{"0"}, 0, 0, "0");
  checkValid(Amount{"0.1"}, 1, 1, "0.1");
  checkValid(Amount{"1234"}, 1, 0, "1234");
  checkValid(Amount{"23.45"}, 1, 2, "23.45");
}

TEST_F(AmountTest, Strange) {
  checkValid(Amount{"000.000"}, 0, 3, "0.000");
  checkValid(Amount{"000.001"}, 1, 3, "0.001");
  checkValid(Amount{"0000000"}, 0, 0, "0");
  checkValid(Amount{"0000009"}, 1, 0, "9");
  checkValid(Amount{"1.00000"}, 1, 5, "1.00000");
}

TEST_F(AmountTest, Invalid) {
  checkInvalid(Amount{"-100"});
  checkInvalid(Amount{"-1.23"});
  checkInvalid(Amount{"0xFF"});
  checkInvalid(Amount{"12.34.56"});
  checkInvalid(Amount{".3456"});
  checkInvalid(Amount{".12.34"});
  checkInvalid(Amount{"0A"});
  checkInvalid(Amount{"1."});
  checkInvalid(Amount{"."});
  checkInvalid(Amount{""});
}


/*
==============================================================================
KratosULFApplication
A library based on:
Kratos
A General Purpose Software for Multi-Physics Finite Element Analysis
Version 1.0 (Released on march 05, 2007).
Copyright 2007
Pooyan Dadvand, Riccardo Rossi, Janosch Stascheit, Felix Nagel
pooyan@cimne.upc.edu
rrossi@cimne.upc.edu
- CIMNE (International Center for Numerical Methods in Engineering),
Gran Capita' s/n, 08034 Barcelona, Spain
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 condition:
Distribution of this code for  any  commercial purpose  is permissible
ONLY BY DIRECT ARRANGEMENT WITH THE COPYRIGHT OWNERS.
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.
==============================================================================
*/

//
//   Project Name:        Kratos
//   Last modified by:    $Author: anonymous $
//   Date:                $Date: 2008-04-07 09:50:09 $
//   Revision:            $Revision: 1.3 $
//
//

// System includes

// External includes

// Project includes
#include <pybind11/pybind11.h>
#include "includes/define.h"
#include "includes/define_python.h"

#include "custom_python/add_custom_strategies_to_python.h"
#include "processes/process.h"
#include "spaces/ublas_space.h"
#include <boost/timer.hpp>

//builder_and_solvers
#include "custom_strategies/builder_and_solvers/residualbased_elimination_quasiincompresible_builder_and_solver.h"
#include "custom_strategies/strategies/modified_linear_strategy.h"
#include "solving_strategies/strategies/solving_strategy.h"
#include "custom_strategies/strategies/runge_kutta_fracstep_GLS_strategy.h"
//schemes
#include "solving_strategies/schemes/scheme.h"
#include "custom_strategies/schemes/residualbased_predictorcorrector_bossak_scheme.h"

//linear solvers
#include "linear_solvers/linear_solver.h"

namespace Kratos
{

namespace Python
{
namespace py = pybind11;

void  AddCustomStrategiesToPython(pybind11::module& m)
{
    typedef UblasSpace<double, CompressedMatrix, Vector> SparseSpaceType;
    typedef UblasSpace<double, Matrix, Vector> LocalSpaceType;

    typedef LinearSolver<SparseSpaceType, LocalSpaceType > LinearSolverType;
    typedef Scheme< SparseSpaceType, LocalSpaceType > BaseSchemeType;

    typedef SolvingStrategy< SparseSpaceType, LocalSpaceType, LinearSolverType > BaseSolvingStrategyType;

    typedef BuilderAndSolver<SparseSpaceType, LocalSpaceType, LinearSolverType> BuilderAndSolverType;
    typedef Scheme< SparseSpaceType, LocalSpaceType > BaseSchemeType;
//			typedef ResidualBasedEliminationBuilderAndSolver<SparseSpaceType, LocalSpaceType, LinearSolverType> ResidualBasedEliminationBuilderAndSolverType;

    //typedef ResidualBasedPredictorCorrectorBossakScheme< SparseSpaceType, LocalSpaceType >   ResidualBasedPredictorCorrectorBossakSchemeType;
    //********************************************************************
    //********************************************************************
    //typedef ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< SparseSpaceType, LocalSpaceType, LinearSolverType, 2> //ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType2D;

    typedef ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< SparseSpaceType, LocalSpaceType, LinearSolverType, 2> ResidualBasedIncompressibleBuilderType2D;

    //py::class_< ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType2D, boost::noncopyable>
//("ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver2D", init< LinearSolverType::Pointer>() )
//
//    //********************************************************************
    //********************************************************************

py::class_< ResidualBasedPredictorCorrectorBossakScheme< SparseSpaceType, LocalSpaceType>,
                    typename ResidualBasedPredictorCorrectorBossakScheme< SparseSpaceType, LocalSpaceType>::Pointer,
                    BaseSchemeType >
                    (m, "ResidualBasedPredictorCorrectorBossakScheme")
                    .def(py::init< double >()
                    );

    py::class_< ResidualBasedIncompressibleBuilderType2D, ResidualBasedIncompressibleBuilderType2D::Pointer, BuilderAndSolverType > (m, "ResidualBasedIncompressibleBuilder2D")
    .def(py::init< LinearSolverType::Pointer>() )
    .def("AssembleLHS", &ResidualBasedIncompressibleBuilderType2D::AssembleLHS )
    .def("AssembleRHS", &ResidualBasedIncompressibleBuilderType2D::AssembleRHS )
    .def("BuildAndSolve", &ResidualBasedIncompressibleBuilderType2D::BuildAndSolve)
    .def("SetUpDofSet", &ResidualBasedIncompressibleBuilderType2D::SetUpDofSet)
    .def("SetUpSystem", &ResidualBasedIncompressibleBuilderType2D::SetUpSystem)
    .def("ResizeAndInitializeVectors", &ResidualBasedIncompressibleBuilderType2D::ResizeAndInitializeVectors)
    .def("Build", &ResidualBasedIncompressibleBuilderType2D::Build)
    .def("Solve", &ResidualBasedIncompressibleBuilderType2D::SystemSolve)
    .def("ConstructMatrixStructure", &ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure)
    .def("ConstructMatrixStructure_Mconsistent", &ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure_Mconsistent)
    .def("ConstructMatrixStructure_DivergenceMatrixD", &ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure_DivergenceMatrixD)
    .def("BuildAuxiliaries", &ResidualBasedIncompressibleBuilderType2D::BuildAuxiliaries)
    .def("AssembleMassMatrices", &ResidualBasedIncompressibleBuilderType2D::AssembleMassMatrices)
    .def("calc_GMinvD_prod", &ResidualBasedIncompressibleBuilderType2D::calc_GMinvD_prod)
    .def("CalculatePreconditionerDiagonalMatrix",  &ResidualBasedIncompressibleBuilderType2D::CalculatePreconditionerDiagonalMatrix)
    .def("calc_prod_precond_vec", &ResidualBasedIncompressibleBuilderType2D::calc_prod_precond_vec)
    .def("ModifyForDirichlet", &ResidualBasedIncompressibleBuilderType2D::ModifyForDirichlet)
    .def("UpdatePressures", &ResidualBasedIncompressibleBuilderType2D::UpdatePressures)
    .def("ReturnDx", &ResidualBasedIncompressibleBuilderType2D::ReturnDx)
    .def("UpdatePressuresNew", &ResidualBasedIncompressibleBuilderType2D::UpdatePressuresNew)
    .def("UpdateAfterProjection", &ResidualBasedIncompressibleBuilderType2D::UpdateAfterProjection)
    .def("ComputePressureAtFreeSurface", &ResidualBasedIncompressibleBuilderType2D::ComputePressureAtFreeSurface)
    .def("SavePressureIteration", &ResidualBasedIncompressibleBuilderType2D::SavePressureIteration)
    .def("FractionalStepProjection", &ResidualBasedIncompressibleBuilderType2D::FractionalStepProjection)
    .def("CalculateNodalPressureForce", &ResidualBasedIncompressibleBuilderType2D::CalculateNodalPressureForce )
    .def("ConvergenceCheck", &ResidualBasedIncompressibleBuilderType2D::ConvergenceCheck)
    //.def("BuildAuxiliariesFSI", &ResidualBasedIncompressibleBuilderType2D::BuildAuxiliariesFSI)
    //.def("ConstructMatrixStructure_Fluid_DivergenceMatrixD",&ResidualBasedIncompressibleBuilderType2D::ConstructMatrixStructure_Fluid_DivergenceMatrixD)

    ;

    typedef ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver< SparseSpaceType, LocalSpaceType, LinearSolverType, 3> ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D;

    py::class_< ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D, ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::Pointer, BuilderAndSolverType > (m,"ResidualBasedEliminationQuasiIncompressibleBuilderAndSolver3D")
    .def(py::init< LinearSolverType::Pointer>() )
    .def("AssembleLHS", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::AssembleLHS )
    .def("AssembleRHS", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::AssembleRHS )
    .def("BuildAndSolve", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::BuildAndSolve)
    .def("SetUpDofSet", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::SetUpDofSet)
    .def("SetUpSystem", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::SetUpSystem)
    .def("ResizeAndInitializeVectors", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ResizeAndInitializeVectors)
    .def("Build", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::Build)
    .def("Solve", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::SystemSolve)
    .def("ConstructMatrixStructure", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConstructMatrixStructure)
    .def("ConstructMatrixStructure_Mconsistent", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConstructMatrixStructure_Mconsistent)
    .def("ConstructMatrixStructure_DivergenceMatrixD", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConstructMatrixStructure_DivergenceMatrixD)
    .def("BuildAuxiliaries", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::BuildAuxiliaries)
    .def("AssembleMassMatrices", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::AssembleMassMatrices)
    .def("calc_GMinvD_prod", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::calc_GMinvD_prod)
    .def("CalculatePreconditionerDiagonalMatrix",  &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::CalculatePreconditionerDiagonalMatrix)
    .def("calc_prod_precond_vec", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::calc_prod_precond_vec)
    .def("ModifyForDirichlet", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ModifyForDirichlet)
    .def("UpdatePressures", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::UpdatePressures)
    .def("ReturnDx", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ReturnDx)
    .def("UpdatePressuresNew", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::UpdatePressuresNew)
    .def("CalculateNodalPressureForce", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::CalculateNodalPressureForce )
    .def("ConvergenceCheck", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ConvergenceCheck)
    .def("UpdateAfterProjection", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::UpdateAfterProjection)
    .def("ComputePressureAtFreeSurface", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::ComputePressureAtFreeSurface)
    .def("SavePressureIteration", &ResidualBasedEliminationQuasiIncompressibleBuilderAndSolverType3D::SavePressureIteration)
    ;

    py::class_< RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >, RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::Pointer, BaseSolvingStrategyType > (m,"RungeKuttaFracStepStrategy2D")
     .def(py::init < ModelPart&, LinearSolverType::Pointer, bool, bool, bool >())
     //.def("SolveStep1", &RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::SolveStep1)
     .def("SolveStep2", &RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::SolveStep2)
     .def("SolveStep3", &RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::SolveStep3)
     //.def("SolveStep_ForwardEuler", &RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::SolveStep_ForwardEuler)
     .def("SolveStep1", &RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::SolveStep1)
     .def("Clear", &RungeKuttaFracStepStrategy < 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::Clear)
     ;

//    py::class_< ResidualBasedSemiEulerianConvectionDiffusionStrategy< SparseSpaceType, LocalSpaceType, LinearSolverType >,
//            ResidualBasedSemiEulerianConvectionDiffusionStrategy< SparseSpaceType, LocalSpaceType, LinearSolverType >::Pointer,
//            BaseSolvingStrategyType >
//            (m,"ResidualBasedSemiEulerianConvectionDiffusionStrategy")
//            .def( init<	ModelPart&, LinearSolverType::Pointer,	bool, int	>() )
//            .def("Clear",&ResidualBasedSemiEulerianConvectionDiffusionStrategy< SparseSpaceType, LocalSpaceType, LinearSolverType >::Clear)
//            ;

    //********************************************************************
    //********************************************************************
    typedef SolvingStrategy< SparseSpaceType, LocalSpaceType, LinearSolverType > BaseSolvingStrategyType;
//strategy base class
    py::class_< BaseSolvingStrategyType, BaseSolvingStrategyType::Pointer >(m,"SolvingStrategy", py::module_local())
    .def(py::init< ModelPart&, bool >() )
    .def("Predict", &BaseSolvingStrategyType::Predict )
    .def("Solve", &BaseSolvingStrategyType::Solve )
    .def("IsConverged", &BaseSolvingStrategyType::IsConverged )
    .def("CalculateOutputData", &BaseSolvingStrategyType::CalculateOutputData )
    .def("SetEchoLevel", &BaseSolvingStrategyType::SetEchoLevel )
    .def("GetEchoLevel", &BaseSolvingStrategyType::GetEchoLevel )
    .def("SetRebuildLevel", &BaseSolvingStrategyType::SetRebuildLevel )
    .def("GetRebuildLevel", &BaseSolvingStrategyType::GetRebuildLevel )
    .def("SetMoveMeshFlag", &BaseSolvingStrategyType::SetMoveMeshFlag )
    .def("MoveMeshFlag", &BaseSolvingStrategyType::MoveMeshFlag )
    .def("MoveMesh", &BaseSolvingStrategyType::MoveMesh )
    .def("Clear", &BaseSolvingStrategyType::Clear )
    //.def("GetModelPart", &BaseSolvingStrategyType::GetModelPart )
    ;
    typedef LapModifiedLinearStrategy< 2, SparseSpaceType, LocalSpaceType, LinearSolverType> LapModifiedLinearStrategy2D;

    py::class_< LapModifiedLinearStrategy2D, LapModifiedLinearStrategy2D::Pointer, BaseSolvingStrategyType >
    (m,"LapModifiedLinearStrategy2D")
     .def(py::init<ModelPart&,BaseSchemeType::Pointer, LinearSolverType::Pointer, bool, bool, bool, bool	>() )
    .def("Solve", &LapModifiedLinearStrategy< 2, SparseSpaceType, LocalSpaceType, LinearSolverType >::Solve )
    ;
    typedef LapModifiedLinearStrategy< 3, SparseSpaceType, LocalSpaceType, LinearSolverType> LapModifiedLinearStrategy3D;

    py::class_< LapModifiedLinearStrategy3D, LapModifiedLinearStrategy3D::Pointer, BaseSolvingStrategyType >
    (m,"LapModifiedLinearStrategy3D")
     .def(py::init<ModelPart&,BaseSchemeType::Pointer, LinearSolverType::Pointer, bool, bool, bool, bool	>() )
    .def("Solve", &LapModifiedLinearStrategy< 3, SparseSpaceType, LocalSpaceType, LinearSolverType >::Solve )
    ;

}

}  // namespace Python.

} // Namespace Kratos


#include <ctime>
#include <vector>
#include <string>
#include <iostream>
#include <iomanip>
#include <fstream>
#include <stdexcept>
#include <sys/stat.h>
#include <algorithm>
#include "qm_model.hpp"
#include "properties.hpp"
#include <armadillo>
#include <unordered_map>

ConfigBlockReader QM_Model::model_reader() {
  ConfigBlockReader reader{"qmmodel"};
  reader.add_entry("model", "avoided-crossing");
  return reader;
}

QM_Model::QM_Model(FileHandle& fh,
                   const arma::uvec& in_qmids,
		   arma::mat& in_qm_crd,
		   arma::mat& in_mm_crd,
		   arma::vec& in_mm_chg,
		   const int charge,
		   const int mult,
		   const int excited_states,
                   const int min_state):
  QMInterface(in_qmids, in_qm_crd, in_mm_crd, in_mm_chg, charge, mult, excited_states, min_state)
{

  if (NQM > 1){
    throw std::logic_error("Model systems only work on the x component of a single atom!");
  }

  if (NMM != 0){
    throw std::logic_error("Model systems don't support QM/MM!");
  }

  ConfigBlockReader reader = model_reader();
  reader.parse(fh);

  {
    std::string model_in;
    reader.get_data("model", model_in);

    if (model_in == "avoided-crossing"){
      model= new AvoidedCrossing();
    }
    else if (model_in == "reflective-avoided-crossing"){
      model= new ReflectiveAvoidedCrossing();
    }
    else{
      throw std::runtime_error("Unknown Model: `" + model_in + "`!");
    }
  }
}

void QM_Model::update(void){
  QMInterface::update();
  // always take the x component of the first atom
  model->update(crd_qm(0));
}

void QM_Model::get_properties(PropMap &props){
  for (QMProperty p: props.keys()){
    switch(p){

    case QMProperty::wfoverlap:{
      arma::mat &U = props.get(QMProperty::wfoverlap);
      U = model->overlaps();
      break;
    }

    case QMProperty::nacvector:{
      arma::mat & nac = props.get(QMProperty::nacvector);
      nac.zeros();

      const arma::uvec &idx = *props.get_idx(QMProperty::nacvector);
      size_t A = idx[0]; size_t B = idx[1];

      nac(0) = model->nacs()(A,B);
      break;
    }

    case QMProperty::qmgradient:{
      arma::mat & g_qm = props.get(QMProperty::qmgradient);
      g_qm.zeros();

      arma::uword surface = 0; //assume ground state
      if (props.has_idx(QMProperty::qmgradient)){
	surface = (*props.get_idx(QMProperty::qmgradient))[0];
      }
      g_qm(0) = model->gradients()(surface);

      break;
    }

    case QMProperty::qmgradient_multi:{
      arma::cube & g_qm = props.get(QMProperty::qmgradient_multi);
      g_qm.zeros();

      arma::uvec surfaces = arma::regspace<arma::uvec>(0,excited_states); //assume all
      if (props.has_idx(QMProperty::qmgradient_multi)){ // specific states
	surfaces = *props.get_idx(QMProperty::qmgradient_multi);
      }

      if (g_qm.n_slices != surfaces.n_elem){
	throw std::range_error("Insufficient space for requested gradients!");
      }

      arma::uword i=0;
      for (arma::uword surface: surfaces){
        g_qm.slice(i)(0) = model->gradients()(surface);
        i++;
      }
      break;
    }

    case QMProperty::energies:{
      arma::vec & energies = props.get(QMProperty::energies);
      energies.zeros();

      if (props.has_idx(QMProperty::energies)){
      	energies = model->energies();
      }
      else{
      	energies = model->energies()(0);
      }
      break;
    }

    case QMProperty::mmgradient: //explicit fall-through
    case QMProperty::mmgradient_multi:
      break;

    default:
      throw std::invalid_argument("Unknown QMProperty!");
      break;
    }
  }
}

// updates overlap and, phase matches the new eigen vectors to the old ones.
void HamiltonianDynamics::update_overlap(arma::mat &evec){
  static arma::mat evec_last = evec;
  overlap = evec_last.t() * evec;

  while (arma::any(overlap.diag() < 0)){ // phase match
    arma::uword c = overlap.diag().index_min();
    evec.col(c) *= -1.0;

    overlap = evec_last.t() * evec;
  }

  evec_last = evec;
}

void HamiltonianDynamics::save_state(double x){
  std::vector<double> state;

  state.push_back( x );
  state.push_back( energy(0) );
  state.push_back( energy(1) );
  state.push_back( gradient(0) );
  state.push_back( gradient(1) );
  state.push_back( overlap(0,1) );
  state.push_back( nac(0,1) );

  std::ofstream stream;
  stream.open(state_file, std::ios::out | std::ios::app | std::ios::binary);
  if (!stream){
    throw std::runtime_error("Cannot open data file!");
  }
  else{
    for (auto e: state){
      stream << e << " ";
    }
    stream << std::endl;
  }
}

void HamiltonianDynamics::scan(double a, double b, int N){
  record = true;

  if ((b-a < 0) || (N < 0)){
    throw::std::logic_error("Can only scan on [a,b) in N>0 steps!");
  }

  const double dx = (b-a)/N;

  for (double x = a; x < b; x += dx){
    update(x);
  }

}

ReflectiveAvoidedCrossing::ReflectiveAvoidedCrossing(void){
  energy.set_size(2);
  gradient.set_size(2);
  nac.set_size(2,2);
  overlap.set_size(2,2);
}

void ReflectiveAvoidedCrossing::update(double x){
  double e2 = A*std::tanh(B*(x+7));
  double e1 = e2 + 2*A*std::tanh(B*x) + 2*A;

  double v = C * std::exp(-1.0*(x+7)*(x+7));

  arma::mat H = {{e1, v },
                 {v, -e2}};

  arma::vec eval;
  arma::mat evec;
  eig_sym(eval, evec, H);

  update_overlap(evec);

  energy = eval;

  // compute gradients
  {
    double de2 = A*B / std::pow(std::cosh(B*(x+7)),2) ;
    double de1 = A*B / std::pow(std::cosh(B*x)    ,2) + de2 ;
    double dv = -C * 2 * (x+7) *  std::exp(-1.0*(x+7)*(x+7));

    // gradient = 0.5*((de1 - de2) +
    //                 ((de2-de1)*(e2-e1) + 2*(e1*de2 + e2*de1) + 4*v*dv) /
    //                 ((e2-e1) + 2 * energy));

    double gl = 0.5 * (de1 - de2);
    double gr = 0.5 * ((de2-de1)*(e2-e1) + 2*(e1*de2 + e2*de1) + 4*v*dv) /
      std::sqrt((e2-e1)*(e2-e1) + 4*e1*e2+4*v*v);

    gradient(0) = gl - gr;
    gradient(1) = gl + gr;
  }

  // compute NAC
  nac = evec.t() * arma::diagmat(gradient) * evec;
  nac.diag().zeros();
  {
    double gap = energy(1) - energy(0);
    nac(0,1) /=  gap;
    nac(1,0) /= -gap;
  }

  if(record){
    save_state(x);
  }
}

AvoidedCrossing::AvoidedCrossing(void){
  energy.set_size(2);
  gradient.set_size(2);
  nac.set_size(2,2);
  overlap.set_size(2,2);
}

void AvoidedCrossing::update(double x){
  double xs = x+7;

  double e = A * std::tanh(B*xs);
  double v = C * std::exp(-1.0*xs*xs);

  arma::mat H = {{e,  v},
                 {v, -e}};

  arma::vec eval;
  arma::mat evec;
  eig_sym(eval, evec, H);

  update_overlap(evec);

  energy = eval;

  // compute gradients
  double f;
  {
    double sech = 1.0 / std::cosh(B*xs);
    f = e*A*B*sech*sech + -2.0*xs*v*v;
  }
  gradient = f / energy;

  // compute NAC
  nac = evec.t() * arma::diagmat(gradient) * evec;
  nac.diag().zeros();
  {
    double gap = energy(1) - energy(0);
    nac(0,1) /=  gap;
    nac(1,0) /= -gap;
  }

  // save a bunch of things
  if(record)
  {
    save_state(x);
  }
}


/*
	This file is part of cpp-ethereum.

	cpp-ethereum is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	cpp-ethereum 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 Public License for more details.

	You should have received a copy of the GNU General Public License
	along with cpp-ethereum.  If not, see <http://www.gnu.org/licenses/>.
*/
/** @file ICAP.cpp
 * @author Gav Wood <i@gavwood.com>
 * @date 2014
 */

#include "ICAP.h"
#include <boost/algorithm/string/case_conv.hpp>
#include <boost/algorithm/string.hpp>
#include <libdevcore/Base64.h>
#include <libdevcore/SHA3.h>
#include "Exceptions.h"
#include "ABI.h"

using namespace std;
using namespace dev;
using namespace dev::eth;

namespace dev
{
    namespace eth
    {

        string ICAP::iban(std::string _c, std::string _d)
        {
            boost::to_upper(_c);
            boost::to_upper(_d);
            auto totStr = _d + _c + "00";
            bigint tot = 0;
            for (char x: totStr)
                if (x >= 'A')
                    tot = tot * 100 + x - 'A' + 10;
                else
                    tot = tot * 10 + x - '0';
            unsigned check = (unsigned)(u256)(98 - tot % 97);
            ostringstream out;
            out << _c << setfill('0') << setw(2) << check << _d;
            return out.str();
        }

        std::pair<string, string> ICAP::fromIBAN(std::string _iban)
        {
            if (_iban.size() < 4)
                return std::make_pair(string(), string());
            boost::to_upper(_iban);
            std::string c = _iban.substr(0, 2);
            std::string d = _iban.substr(4);
            if (iban(c, d) != _iban)
                return std::make_pair(string(), string());
            return make_pair(c, d);
        }

        Secret ICAP::createDirect()
        {
            Secret ret;
            while (true)
            {
                ret = Secret::random();
                if (!toAddress(ret)[0])
                    return ret;
            }
        }

        ICAP ICAP::decoded(std::string const &_encoded)
        {
            ICAP ret;
            std::string country;
            std::string data;
            std::tie(country, data) = fromIBAN(_encoded);
            if (country != "XE")
                BOOST_THROW_EXCEPTION(InvalidICAP());
            if (data.size() == 30 || data.size() == 31)
            {
                ret.m_type = Direct;
                // Direct ICAP
                ret.m_direct = fromBase36<Address::size>(data);
            } else if (data.size() == 16)
            {
                ret.m_type = Indirect;
                ret.m_asset = data.substr(0, 3);
                if (ret.m_asset == "XET" || ret.m_asset == "ETH")
                {
                    ret.m_institution = data.substr(3, 4);
                    ret.m_client = data.substr(7);
                } else
                    BOOST_THROW_EXCEPTION(InvalidICAP());
            } else
                BOOST_THROW_EXCEPTION(InvalidICAP());

            return ret;
        }

        std::string ICAP::encoded() const
        {
            if (m_type == Direct)
            {
                std::string d = toBase36<Address::size>(m_direct);
                while (d.size() < 30)        // always 34, sometimes 35.
                    d = "0" + d;
                return iban("XE", d);
            } else if (m_type == Indirect)
            {
                if (
                        m_asset.find_first_not_of("qwertyuiopasdfghjklzxcvbnmQWERTYUIOPASDFGHJKLZXCVBNM1234567890") !=
                        string::npos ||
                        m_institution.find_first_not_of(
                                "qwertyuiopasdfghjklzxcvbnmQWERTYUIOPASDFGHJKLZXCVBNM1234567890") != string::npos ||
                        m_client.find_first_not_of("qwertyuiopasdfghjklzxcvbnmQWERTYUIOPASDFGHJKLZXCVBNM1234567890") !=
                        string::npos ||
                        m_asset.size() != 3 ||
                        (boost::to_upper_copy(m_asset) != "XET" && boost::to_upper_copy(m_asset) != "ETH") ||
                        m_institution.size() != 4 ||
                        m_client.size() != 9
                        )
                    BOOST_THROW_EXCEPTION(InvalidICAP());
                return iban("XE", m_asset + m_institution + m_client);
            } else
                BOOST_THROW_EXCEPTION(InvalidICAP());
        }

        pair<Address, bytes> ICAP::lookup(std::function<bytes(Address, bytes)> const &_call, Address const &_reg) const
        {
            auto resolve = [&](string const &s)
            {
                /*		vector<string> ss;
                        boost::algorithm::split(ss, s, boost::is_any_of("/"));
                        Address r = _reg;
                        for (unsigned i = 0; i < ss.size() - 1; ++i)
                            r = abiOut<Address>(_call(r, abiIn("subRegistrar(bytes32)", ss[i])));*/
                return abiOut<Address>(_call(_reg, abiIn("addr(string)", s)));
            };
            if (m_asset == "XET")
            {
                Address a = resolve(m_institution);
                bytes d = abiIn("deposit(uint64)", fromBase36<8>(m_client));
                return make_pair(a, d);
            }
            /*	else if (m_asset == "ETH")
                {
                    if (m_institution[0] != 'X')
                        return make_pair(resolve(m_institution + "/" + m_client), bytes());
                    else if (m_institution == "XREG")
                        return make_pair(resolve(m_client), bytes());
                    else
                        BOOST_THROW_EXCEPTION(InterfaceNotSupported("ICAP::lookup(), bad institution"));
                }*/
            BOOST_THROW_EXCEPTION(InterfaceNotSupported("ICAP::lookup(), bad asset"));
        }

    }
}


/***
 *  Software License Agreement: BSD 3-Clause License
 *
 * Copyright (c) 2022, NMMI
 * 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 the copyright holder nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

// ----------------------------------------------------------------------------

/**
 * \file      head_manager.cpp
 *
 * \author       _Centro di Ricerca "E.Piaggio"_
 * \author       _Istituto Italiano di Tecnologia, Soft Robotics for Human Cooperation and Rehabilitation Lab_
 **/
// ----------------------------------------------------------------------------

#include "ros/ros.h"
#include <eigen3/Eigen/Eigen>
#include <math.h>
#include <vector>
#include <string>
#include <kdl/chain.hpp>
#include <kdl/chainfksolver.hpp>
#include <kdl/frames_io.hpp>
#include <kdl/chainjnttojacsolver.hpp>
#include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chaindynparam.hpp>
#include <kdl/frames.hpp>
#include <kdl/jacobian.hpp>
#include <kdl/jntarray.hpp>
#include <boost/scoped_ptr.hpp>
#include <tf/transform_broadcaster.h>

#include <geometry_msgs/Pose.h>
#include "std_msgs/Float64.h"
#include "std_msgs/Float32MultiArray.h"
#include <trajectory_msgs/JointTrajectory.h>

#include <sensor_msgs/JointState.h>

using namespace KDL;
using namespace std;

#define MAX_STIFF 0.3

Frame ref_frame;
Eigen::VectorXd ref_twist(6);
Eigen::VectorXd ref_stiffness(6);
int arm_side;
std::string ns;
Eigen::Matrix4d T_b_0;
Eigen::Matrix3d R_o_b;
ros::Time cmd_time, cmd_time_old;

/*---------------------------------------------------------------------*
 * POSTURE CALLBACK                                                     *
 *                                                                      *
 *----------------------------------------------------------------------*/
void posture__Callback(const geometry_msgs::Pose::ConstPtr &msg)
{
  Eigen::Quaterniond ref_quat;
  static Eigen::Quaterniond old_quat;
  double sign_check;
  Eigen::Vector3d r_p;
  Eigen::Matrix3d r_M;
  static tf::TransformBroadcaster br;
  tf::Transform transform;

  // r_p << msg->position.x, msg->position.y, msg->position.z;
  // r_p = r_p*0.38;
  ref_quat.x() = msg->orientation.x;
  ref_quat.y() = msg->orientation.y;
  ref_quat.z() = msg->orientation.z;
  ref_quat.w() = msg->orientation.w;

  sign_check = ref_quat.w() * old_quat.w() + ref_quat.x() * old_quat.x() + ref_quat.y() * old_quat.y() + ref_quat.z() * old_quat.z();
  if (sign_check < 0.0)
  {
    ref_quat.w() = -ref_quat.w();
    ref_quat.vec() = -ref_quat.vec();
  }
  old_quat = ref_quat;

  ref_quat = (R_o_b * ref_quat * R_o_b.transpose()) * T_b_0.block<3, 3>(0, 0).transpose();
  r_p = R_o_b * r_p + T_b_0.block<3, 1>(0, 3);
  r_M = ref_quat;

  ref_frame = Frame(Rotation(r_M(0, 0), r_M(0, 1), r_M(0, 2), r_M(1, 0), r_M(1, 1), r_M(1, 2), r_M(2, 0), r_M(2, 1), r_M(2, 2)), Vector(r_p(0), r_p(1), r_p(2)));

  transform.setOrigin(tf::Vector3(r_p(0), r_p(1), r_p(2)));
  transform.setRotation(tf::Quaternion(ref_quat.x(), ref_quat.y(), ref_quat.z(), ref_quat.w()));
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "root_link", ns + "_ref_head"));

  cmd_time_old = cmd_time;
  cmd_time = ros::Time::now();

  // cout << " " << endl;
  // cout << ref_frame.M << endl;
  // cout << " " << endl;
  // cout << ref_frame.p << endl;
}

/*---------------------------------------------------------------------*
 * STIFFNESS CALLBACK                                                   *
 *                                                                      *
 *----------------------------------------------------------------------*/
void stiffness__Callback(const std_msgs::Float32MultiArray::ConstPtr &msg)
{
  for (int i = 0; i < 2; i++)
    ref_stiffness(i) = msg->data[i];
}

/*---------------------------------------------------------------------*
 * MAIN                                                                 *
 *                                                                      *
 *----------------------------------------------------------------------*/
int main(int argc, char **argv)
{
  // ------------------------------------------------------------------------------------- Init Var
  // --- node param ---
  std::string chain_topic;
  std::string pose_ref_topic;
  std::string stiff_ref_topic;
  std::string phantom_arm_topic;
  std::string ref_head_topic;
  int run_freq;
  ros::Subscriber sub_posture;
  ros::Subscriber sub_stiffness;
  ros::Publisher pub_cart_ref;
  ros::Publisher pub_phantom;
  ros::Publisher pub_head_ref;
  geometry_msgs::Pose cart_ref_msg;
  sensor_msgs::JointState phantom_msg;
  char buffer[50];

  sensor_msgs::JointState head_ref_msg;

  // --- kinematics ---
  std::vector<double> DH;
  std::vector<double> DH_Xtr;
  std::vector<double> DH_Xrot;
  std::vector<double> DH_Ztr;
  std::vector<double> DH_Zrot;
  std::vector<double> T_t2s;
  std::vector<double> T_o2t;
  std::vector<double> R_o2b;
  std::vector<double> q_min;
  std::vector<double> q_max;
  std::vector<int> qbmove_tf_ids;
  int softhand_tf_id;

  KDL::Chain chain;
  boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver;
  boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver;

  Eigen::VectorXd qMax_left(2);
  Eigen::VectorXd qMin_left(2);
  Eigen::VectorXd qMax_right(2);
  Eigen::VectorXd qMin_right(2);

  KDL::JntArray q;
  KDL::Jacobian JA_kdl;
  KDL::Frame act_frame;
  KDL::Twist err_twist;
  Eigen::VectorXd err_post(6);
  Eigen::VectorXd x_post(6);
  Eigen::MatrixXd K_v(6, 6);
  K_v << 50 * Eigen::MatrixXd::Identity(6, 6);
  Eigen::MatrixXd JA(6, 2);
  Eigen::MatrixXd JA_pinv(2, 6);
  Eigen::MatrixXd K_d(6, 6);
  K_d << 0.1 * Eigen::MatrixXd::Identity(6, 6);
  double k_0;
  k_0 = 2;

  Eigen::VectorXd eq_dot(2);
  Eigen::VectorXd eq(2);
  Eigen::VectorXd eq_f(2);

  double arm_l;
  Eigen::Quaterniond act_quat;

  unsigned long int msg_seq(0);
  double stiffn;

  ros::Duration max_cmd_time = ros::Duration(1);
  ros::Duration filt_time = ros::Duration(5);
  ros::Duration max_cmd_latency = ros::Duration(1);
  ros::Time start_f_time;
  int act_bp(1);
  double alpha(1);

  // ------------------------------------------------------------------------------------- Init node
  ros::init(argc, argv, "head_manager");
  ros::NodeHandle n;
  ns = ros::this_node::getNamespace();
  ns = ns.substr(1, ns.length() - 1);

  // --- Rviz ---
  tf::TransformBroadcaster ik_br;
  tf::Transform ik_tf;

  // ------------------------------------------------------------------------------------- Check/save args
  n.getParam("/head/DH_Xtr", DH_Xtr);
  n.getParam("/head/DH_Xrot", DH_Xrot);
  n.getParam("/head/DH_Ztr", DH_Ztr);
  n.getParam("/head/DH_Zrot", DH_Zrot);
  n.getParam("/head/T_t2s", T_t2s);
  n.getParam("/head/T_o2t", T_o2t);
  n.getParam("/head/R_o2b", R_o2b);
  n.getParam("/head/q_min", q_min);
  n.getParam("/head/q_max", q_max);
  n.getParam("/head/qbmove_tf_ids", qbmove_tf_ids);
  n.getParam("/head/pose_ref_topic", pose_ref_topic);
  n.getParam("/head/stiff_ref_topic", stiff_ref_topic);
  n.getParam("/head/phantom_arm_topic", phantom_arm_topic);
  n.getParam("/head/ref_head_topic", ref_head_topic);
  n.getParam("/head/active_back_pos", act_bp);

  run_freq = 50;
  n.getParam("/head/head_frequency", run_freq); // Override if configured
  ros::Rate loop_rate(run_freq);

  // ------------------------------------------------------------------------------------- Kinematics

  chain.addSegment(Segment(Joint(Joint::None), Frame(Rotation(T_t2s[0], T_t2s[4], T_t2s[8], T_t2s[1], T_t2s[5], T_t2s[9], T_t2s[2], T_t2s[6], T_t2s[10]),
                                                     Vector(T_t2s[3], T_t2s[7], T_t2s[11]))));
  chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(DH_Xtr[0], DH_Xrot[0], DH_Ztr[0], DH_Zrot[0])));
  chain.addSegment(Segment(Joint(Joint::RotZ), Frame::DH(DH_Xtr[1], DH_Xrot[1], DH_Ztr[1], DH_Zrot[1])));

  T_b_0 << T_o2t[0], T_o2t[1], T_o2t[2], T_o2t[3],
      T_o2t[4], T_o2t[5], T_o2t[6], T_o2t[7],
      T_o2t[8], T_o2t[9], T_o2t[10], T_o2t[11],
      T_o2t[12], T_o2t[13], T_o2t[14], T_o2t[15];

  R_o_b << R_o2b[0], R_o2b[1], R_o2b[2],
      R_o2b[3], R_o2b[4], R_o2b[5],
      R_o2b[6], R_o2b[7], R_o2b[8];

  jnt_to_pose_solver.reset(new KDL::ChainFkSolverPos_recursive(chain));
  jnt_to_jac_solver.reset(new KDL::ChainJntToJacSolver(chain));

  q.resize(chain.getNrOfJoints());
  JA_kdl.resize(chain.getNrOfJoints());
  KDL::SetToZero(q);

  jnt_to_pose_solver->JntToCart(q, ref_frame);
  jnt_to_jac_solver->JntToJac(q, JA_kdl);
  for (int i = 0; i < 6; i++)
  {
    for (int j = 0; j < 2; j++)
      if (fabs(JA_kdl(i, j)) > 0.000001)
        JA(i, j) = JA_kdl(i, j);
      else
        JA(i, j) = 0;
  }

  JA_pinv = JA.transpose() * ((JA * JA.transpose() + K_d).inverse());

  ref_twist << 0, 0, 0, 0, 0, 0;

  eq << 0, 0;
  eq_f << 0, 0;

  // ------------------------------------------------------------------------------------- Subscribe to topics
  sub_posture = n.subscribe(pose_ref_topic, 1, posture__Callback);
  sub_stiffness = n.subscribe(stiff_ref_topic, 1, stiffness__Callback);

  // ------------------------------------------------------------------------------------- Published topics
  // ros::Publisher    	pub_inv_kin		= n.advertise<qb_interface::cubeEq_Preset>(target_chain + "_eq_pre", 1000);
  // pub_cart_ref	= n.advertise<geometry_msgs::Pose>(chain_topic, 1);
  pub_head_ref = n.advertise<sensor_msgs::JointState>(ref_head_topic, 1);
  pub_phantom = n.advertise<sensor_msgs::JointState>(phantom_arm_topic, 1);

  stiffn = MAX_STIFF * 0.9;

  cmd_time = ros::Time::now();
  cmd_time_old = ros::Time::now();

  // ------------------------------------------------------------------------------------- MAIN LOOP
  while (ros::ok())
  {
    // --- Inverse Kinematics ---
    jnt_to_pose_solver->JntToCart(q, act_frame);
    jnt_to_jac_solver->JntToJac(q, JA_kdl);
    for (int i = 0; i < 6; i++)
    {
      for (int j = 0; j < 2; j++)
        if (fabs(JA_kdl(i, j)) > 0.000001)
          JA(i, j) = JA_kdl(i, j);
        else
          JA(i, j) = 0;
    }

    err_twist = KDL::diff(act_frame, ref_frame);

    err_post << err_twist[0], err_twist[1], err_twist[2], err_twist[3], err_twist[4], err_twist[5];
    err_post = K_v * err_post;

    JA_pinv = JA.transpose() * ((JA * JA.transpose() + K_d).inverse());

    eq_dot = JA_pinv * err_post;

    eq += eq_dot / run_freq;

    // Back position
    if (act_bp == 1 && (ros::Time::now() - cmd_time > max_cmd_time))
    {
      eq << 0, 0;
      start_f_time = ros::Time::now();
      // cout << "1  " << eq_f.transpose() << endl;
      alpha = 1;
    }

    // Check latency between msgs
    // if (cmd_time-cmd_time_old > max_cmd_latency){
    // 	start_f_time = ros::Time::now();
    // 	// cout << "2  " << cmd_time << endl;
    // 	// cout << "2  " << cmd_time_old << endl;
    // }

    // Filtering position
    if (ros::Time::now() - start_f_time < filt_time)
    {
      alpha -= 1 / (filt_time.toSec() * run_freq);
      if (alpha < 0)
        alpha = 0;
      eq_f = alpha * eq_f + (1 - alpha) * eq;
      // cout << "3  " << eq_f.transpose() << endl;
    }
    else
      eq_f = eq;

    for (int i = 0; i < 2; i++)
    {
      if (eq_f(i) > q_max[i])
      {
        eq_f(i) = q_max[i];
      }
      if (eq_f(i) < q_min[i])
      {
        eq_f(i) = q_min[i];
      }

      q(i) = eq_f(i);
      eq(i) = eq_f(i);
    }
    act_frame.M.GetQuaternion(act_quat.x(), act_quat.y(), act_quat.z(), act_quat.w());

    // --- publish all messages ---
    // Rviz TF:
    ik_tf.setOrigin(tf::Vector3(act_frame.p[0], act_frame.p[1], act_frame.p[2]));
    ik_tf.setRotation(tf::Quaternion(act_quat.x(), act_quat.y(), act_quat.z(), act_quat.w()));
    ik_br.sendTransform(tf::StampedTransform(ik_tf, ros::Time::now(), "root_link", ns + "_ego_ik"));

    head_ref_msg.name.resize(chain.getNrOfJoints());
    head_ref_msg.position.resize(chain.getNrOfJoints());

    head_ref_msg.name[0] = "neck_0";
    head_ref_msg.name[1] = "neck_1";
    head_ref_msg.position[0] = q(0);
    head_ref_msg.position[1] = q(1);

    // Rviz model:
    phantom_msg.name.resize(chain.getNrOfJoints());
    phantom_msg.position.resize(chain.getNrOfJoints());
    for (int i = 0; i < 2; i++)
    {
      sprintf(buffer, "phantom_cube%d_shaft_joint", qbmove_tf_ids[i]);
      phantom_msg.name[i] = buffer;
      phantom_msg.position[i] = q(i);
    }

    pub_head_ref.publish(head_ref_msg);
    pub_phantom.publish(phantom_msg);

    // // --- cycle ---
    ros::spinOnce();
    loop_rate.sleep();
  }
}


//------------------------------------------------------------------------------
/*
    This file is part of cbcd: https://github.com/cbc/cbcd
    Copyright (c) 2012, 2013 cbc Labs Inc.

    Permission to use, copy, modify, and/or distribute this software for any
    purpose  with  or without fee is hereby granted, provided that the above
    copyright notice and this permission notice appear in all copies.

    THE  SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
    WITH  REGARD  TO  THIS  SOFTWARE  INCLUDING  ALL  IMPLIED  WARRANTIES  OF
    MERCHANTABILITY  AND  FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
    ANY  SPECIAL ,  DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
    WHATSOEVER  RESULTING  FROM  LOSS  OF USE, DATA OR PROFITS, WHETHER IN AN
    ACTION  OF  CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
    OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
//==============================================================================

#include <BeastConfig.h>
#include <cbc/protocol/PublicKey.h>
#include <cbc/protocol/digest.h>
#include <cbc/protocol/impl/secp256k1.h>
#include <cbc/basics/contract.h>
#include <cbc/basics/strHex.h>
#include <cbc/beast/core/ByteOrder.h>
#include <boost/multiprecision/cpp_int.hpp>
#include <ed25519-donna/ed25519.h>
#include <type_traits>

namespace cbc {

std::ostream&
operator<<(std::ostream& os, PublicKey const& pk)
{
    os << strHex(pk.data(), pk.size());
    return os;
}

using uint264 = boost::multiprecision::number<
    boost::multiprecision::cpp_int_backend<
        264, 264, boost::multiprecision::signed_magnitude,
            boost::multiprecision::unchecked, void>>;

template<>
boost::optional<PublicKey>
parseBase58 (TokenType type, std::string const& s)
{
    auto const result =
        decodeBase58Token(s, type);
    if (result.empty())
        return boost::none;
    if (result.size() != 33)
        return boost::none;
    return PublicKey(makeSlice(result));
}

//------------------------------------------------------------------------------

// Parse a length-prefixed number
//  Format: 0x02 <length-byte> <number>
static
boost::optional<Slice>
sigPart (Slice& buf)
{
    if (buf.size() < 3 || buf[0] != 0x02)
        return boost::none;
    auto const len = buf[1];
    buf += 2;
    if (len > buf.size() || len < 1 || len > 33)
        return boost::none;
    // Can't be negative
    if ((buf[0] & 0x80) != 0)
        return boost::none;
    if (buf[0] == 0)
    {
        // Can't be zero
        if (len == 1)
            return boost::none;
        // Can't be padded
        if ((buf[1] & 0x80) == 0)
            return boost::none;
    }
    boost::optional<Slice> number =
        Slice(buf.data(), len);
    buf += len;
    return number;
}

static
std::string
sliceToHex (Slice const& slice)
{
    std::string s;
    if (slice[0] & 0x80)
    {
        s.reserve(2 * (slice.size() + 2));
        s = "0x00";
    }
    else
    {
        s.reserve(2 * (slice.size() + 1));
        s = "0x";
    }
    for(int i = 0; i < slice.size(); ++i)
    {
        s += "0123456789ABCDEF"[((slice[i]&0xf0)>>4)];
        s += "0123456789ABCDEF"[((slice[i]&0x0f)>>0)];
    }
    return s;
}

/** Determine whether a signature is canonical.
    Canonical signatures are important to protect against signature morphing
    attacks.
    @param vSig the signature data
    @param sigLen the length of the signature
    @param strict_param whether to enforce strictly canonical semantics

    @note For more details please see:
    https://cbc.com/wiki/Transaction_Malleability
    https://bitcointalk.org/index.php?topic=8392.msg127623#msg127623
    https://github.com/sipa/bitcoin/commit/58bc86e37fda1aec270bccb3df6c20fbd2a6591c
*/
boost::optional<ECDSACanonicality>
ecdsaCanonicality (Slice const& sig)
{
    static uint264 const G(
        "0xFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFFEBAAEDCE6AF48A03BBFD25E8CD0364141");

    // The format of a signature should be:
    // <30> <len> [ <02> <lenR> <R> ] [ <02> <lenS> <S> ]
    if ((sig.size() < 8) || (sig.size() > 72))
        return boost::none;
    if ((sig[0] != 0x30) || (sig[1] != (sig.size() - 2)))
        return boost::none;
    Slice p = sig + 2;
    auto r = sigPart(p);
    auto s = sigPart(p);
    if (! r || ! s || ! p.empty())
        return boost::none;

    uint264 R(sliceToHex(*r));
    uint264 S(sliceToHex(*s));

    if (R >= G)
        return boost::none;
    if (S >= G)
        return boost::none;
    // (R,S) and (R,G-S) are canonical,
    // but is fully canonical when S <= G-S
    auto const Sp = G - S;
    if (S > Sp)
        return ECDSACanonicality::canonical;
    return ECDSACanonicality::fullyCanonical;
}

static
bool
ed25519Canonical (Slice const& sig)
{
    if (sig.size() != 64)
        return false;
    // Big-endian Order, the Ed25519 subgroup order
    std::uint8_t const Order[] =
    {
        0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
        0x14, 0xDE, 0xF9, 0xDE, 0xA2, 0xF7, 0x9C, 0xD6,
        0x58, 0x12, 0x63, 0x1A, 0x5C, 0xF5, 0xD3, 0xED,
    };
    // Take the second half of signature
    // and byte-reverse it to big-endian.
    auto const le = sig.data() + 32;
    std::uint8_t S[32];
    std::reverse_copy(le, le + 32, S);
    // Must be less than Order
    return std::lexicographical_compare(
        S, S + 32, Order, Order + 32);
}

//------------------------------------------------------------------------------

PublicKey::PublicKey (Slice const& slice)
{
    if(! publicKeyType(slice))
        LogicError("PublicKey::PublicKey invalid type");
    size_ = slice.size();
    std::memcpy(buf_, slice.data(), slice.size());
}

PublicKey::PublicKey (PublicKey const& other)
    : size_ (other.size_)
{
    std::memcpy(buf_, other.buf_, size_);
};

PublicKey&
PublicKey::operator=(
    PublicKey const& other)
{
    size_ = other.size_;
    std::memcpy(buf_, other.buf_, size_);
    return *this;
}

//------------------------------------------------------------------------------

boost::optional<KeyType>
publicKeyType (Slice const& slice)
{
    if (slice.size() == 33 &&
            slice[0] == 0xED)
        return KeyType::ed25519;
    if (slice.size() == 33 &&
        (slice[0] == 0x02 ||
            slice[0] == 0x03))
        return KeyType::secp256k1;
    return boost::none;
}

bool
verifyDigest (PublicKey const& publicKey,
    uint256 const& digest,
    Slice const& sig,
    bool mustBeFullyCanonical)
{
    if (publicKeyType(publicKey) != KeyType::secp256k1)
        LogicError("sign: secp256k1 required for digest signing");
    auto const canonicality = ecdsaCanonicality(sig);
    if (! canonicality)
        return false;
    if (mustBeFullyCanonical &&
        (*canonicality != ECDSACanonicality::fullyCanonical))
        return false;

    secp256k1_pubkey pubkey_imp;
    if(secp256k1_ec_pubkey_parse(
            secp256k1Context(),
            &pubkey_imp,
            reinterpret_cast<unsigned char const*>(
                publicKey.data()),
            publicKey.size()) != 1)
        return false;

    secp256k1_ecdsa_signature sig_imp;
    if(secp256k1_ecdsa_signature_parse_der(
            secp256k1Context(),
            &sig_imp,
            reinterpret_cast<unsigned char const*>(
                sig.data()),
            sig.size()) != 1)
        return false;
    if (*canonicality != ECDSACanonicality::fullyCanonical)
    {
        secp256k1_ecdsa_signature sig_norm;
        if(secp256k1_ecdsa_signature_normalize(
                secp256k1Context(),
                &sig_norm,
                &sig_imp) != 1)
            return false;
        return secp256k1_ecdsa_verify(
            secp256k1Context(),
            &sig_norm,
            reinterpret_cast<unsigned char const*>(
                digest.data()),
            &pubkey_imp) == 1;
    }
    return secp256k1_ecdsa_verify(
        secp256k1Context(),
        &sig_imp,
        reinterpret_cast<unsigned char const*>(
            digest.data()),
        &pubkey_imp) == 1;
}

bool
verify (PublicKey const& publicKey,
    Slice const& m,
    Slice const& sig,
    bool mustBeFullyCanonical)
{
    if (auto const type = publicKeyType(publicKey))
    {
        if (*type == KeyType::secp256k1)
        {
            return verifyDigest (publicKey,
                sha512Half(m), sig, mustBeFullyCanonical);
        }
        else if (*type == KeyType::ed25519)
        {
            if (! ed25519Canonical(sig))
                return false;

            // We internally prefix Ed25519 keys with a 0xED
            // byte to distinguish them from secp256k1 keys
            // so when verifying the signature, we need to
            // first strip that prefix.
            return ed25519_sign_open(
                m.data(), m.size(), publicKey.data() + 1,
                    sig.data()) == 0;
        }
    }
    return false;
}

NodeID
calcNodeID (PublicKey const& pk)
{
    ripesha_hasher h;
    h(pk.data(), pk.size());
    auto const digest = static_cast<
        ripesha_hasher::result_type>(h);
    static_assert(NodeID::bytes ==
        sizeof(ripesha_hasher::result_type), "");
    NodeID result;
    std::memcpy(result.data(),
        digest.data(), digest.size());
    return result;
}

} // cbc


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

  @copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIST_S_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_DIST_S_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/constant/valmax.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/function/dist.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;
  BOOST_DISPATCH_OVERLOAD_IF ( dist_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_< bd::signed_<A0>, X>
                          , bs::pack_< bd::signed_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &
                                    , A0 a0, A0 a1
                                    ) const BOOST_NOEXCEPT
    {
      A0 tmp = dist(a0, a1);
      return if_else(is_ltz(tmp), Valmax<A0>(), tmp);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( dist_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_< bd::unsigned_<A0>, X >
                          , bs::pack_< bd::unsigned_<A0>, X >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &
                                    , A0 a0, A0 a1
                                    ) const BOOST_NOEXCEPT
    {
      return dist(a0, a1);
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( dist_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::saturated_tag
                          , bs::pack_<bd::floating_<A0>, X >
                          , bs::pack_<bd::floating_<A0>, X >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const saturated_tag &
                                    , A0 a0, A0 a1
                                    ) const BOOST_NOEXCEPT
    {
      return dist(a0, a1);
    }
  };
} } }

#endif


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2012 - 2013   MetaScale SAS
//
//          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
//==============================================================================
#ifndef BOOST_SIMD_OPERATOR_FUNCTIONS_OPTIMIZE_MINUS_HPP_INCLUDED
#define BOOST_SIMD_OPERATOR_FUNCTIONS_OPTIMIZE_MINUS_HPP_INCLUDED

#include <boost/simd/operator/functions/minus.hpp>
#include <boost/simd/operator/functions/multiplies.hpp>
#include <boost/simd/include/functions/fms.hpp>
#include <boost/simd/include/functions/fnms.hpp>
#include <boost/dispatch/dsl/category.hpp>
#include <boost/dispatch/functor/preprocessor/call.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_DISPATCH_IMPLEMENT         ( minus_ , tag::formal_, (D)(A0)(A1)
                                   , ((node_<A0, boost::simd::tag::multiplies_, mpl::long_<2> , D>))(unspecified_<A1>)
                                   )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      fms(boost::proto::child_c<0>(a0), boost::proto::child_c<1>(a0), a1)
    )
  };

  BOOST_DISPATCH_IMPLEMENT         ( minus_ , tag::formal_, (D)(A0)(A1)
                                   , (unspecified_<A0>)((node_<A1, boost::simd::tag::multiplies_, mpl::long_<2> , D>))
                                   )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      fnms(boost::proto::child_c<0>(a1), boost::proto::child_c<1>(a1), a0)
    )
  };

  BOOST_DISPATCH_IMPLEMENT         ( minus_ , tag::formal_, (D)(A0)(A1)
                                   , ((node_<A0, boost::simd::tag::multiplies_, mpl::long_<2> , D>))((node_<A1, boost::simd::tag::multiplies_, mpl::long_<2> , D>))
                                   )
  {
    BOOST_DISPATCH_RETURNS(2, (A0 const& a0, A1 const& a1),
      fnms(boost::proto::child_c<0>(a1), boost::proto::child_c<1>(a1), a0)
    )
  };
} } }

#endif


// Copyright (C) 2017  Davis E. King (davis@dlib.net)
// License: Boost Software License   See LICENSE.txt for the full license.

#include <dlib/python.h>
#include <boost/shared_ptr.hpp>
#include <dlib/matrix.h>
#include <boost/python/slice.hpp>
#include <dlib/geometry/vector.h>
#include <dlib/dnn.h>
#include <dlib/image_transforms.h>
#include "indexing.h"

using namespace dlib;
using namespace std;
using namespace boost::python;

typedef matrix<double,0,1> cv;

class face_recognition_model_v1
{

public:

    face_recognition_model_v1(const std::string& model_filename)
    {
        deserialize(model_filename) >> net;

        cropper = make_shared<random_cropper>();
        cropper->set_chip_dims(150,150);
        cropper->set_randomly_flip(true);
        cropper->set_max_object_size(0.99999);
        cropper->set_background_crops_fraction(0);
        cropper->set_min_object_size(0.97);
        cropper->set_translate_amount(0.02);
        cropper->set_max_rotation_degrees(3);
    }

    matrix<double,0,1> compute_face_descriptor (
        object img,
        const full_object_detection& face,
        const int num_jitters
    )
    {
        std::vector<full_object_detection> faces(1, face);
        return compute_face_descriptors(img, faces, num_jitters)[0];
    }

    std::vector<matrix<double,0,1>> compute_face_descriptors (
        object img,
        const std::vector<full_object_detection>& faces,
        const int num_jitters
    )
    {
        if (!is_rgb_python_image(img))
            throw dlib::error("Unsupported image type, must be RGB image.");

        for (auto& f : faces)
        {
            if (f.num_parts() != 68)
                throw dlib::error("The full_object_detection must use the iBUG 300W 68 point face landmark style.");
        }

        std::vector<chip_details> dets;
        for (auto& f : faces)
            dets.push_back(get_face_chip_details(f, 150, 0.25));
        dlib::array<matrix<rgb_pixel>> face_chips;
        extract_image_chips(numpy_rgb_image(img), dets, face_chips);

        std::vector<matrix<double,0,1>> face_descriptors;
        face_descriptors.reserve(face_chips.size());

        if (num_jitters <= 1)
        {
            // extract descriptors and convert from float vectors to double vectors
            for (auto& d : net(face_chips,16))
                face_descriptors.push_back(matrix_cast<double>(d));
        }
        else
        {
            for (auto& fimg : face_chips)
                face_descriptors.push_back(matrix_cast<double>(mean(mat(net(jitter_image(fimg,num_jitters),16)))));
        }

        return face_descriptors;
    }

private:

    std::shared_ptr<random_cropper> cropper;

    std::vector<matrix<rgb_pixel>> jitter_image(
        const matrix<rgb_pixel>& img,
        const int num_jitters
    )
    {
        std::vector<mmod_rect> raw_boxes(1), ignored_crop_boxes;
        raw_boxes[0] = shrink_rect(get_rect(img),3);
        std::vector<matrix<rgb_pixel>> crops;

        matrix<rgb_pixel> temp;
        for (int i = 0; i < num_jitters; ++i)
        {
            (*cropper)(img, raw_boxes, temp, ignored_crop_boxes);
            crops.push_back(move(temp));
        }
        return crops;
    }

    template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
    using residual = add_prev1<block<N,BN,1,tag1<SUBNET>>>;

    template <template <int,template<typename>class,int,typename> class block, int N, template<typename>class BN, typename SUBNET>
    using residual_down = add_prev2<avg_pool<2,2,2,2,skip1<tag2<block<N,BN,2,tag1<SUBNET>>>>>>;

    template <int N, template <typename> class BN, int stride, typename SUBNET>
    using block  = BN<con<N,3,3,1,1,relu<BN<con<N,3,3,stride,stride,SUBNET>>>>>;

    template <int N, typename SUBNET> using ares      = relu<residual<block,N,affine,SUBNET>>;
    template <int N, typename SUBNET> using ares_down = relu<residual_down<block,N,affine,SUBNET>>;

    template <typename SUBNET> using alevel0 = ares_down<256,SUBNET>;
    template <typename SUBNET> using alevel1 = ares<256,ares<256,ares_down<256,SUBNET>>>;
    template <typename SUBNET> using alevel2 = ares<128,ares<128,ares_down<128,SUBNET>>>;
    template <typename SUBNET> using alevel3 = ares<64,ares<64,ares<64,ares_down<64,SUBNET>>>>;
    template <typename SUBNET> using alevel4 = ares<32,ares<32,ares<32,SUBNET>>>;

    using anet_type = loss_metric<fc_no_bias<128,avg_pool_everything<
                                alevel0<
                                alevel1<
                                alevel2<
                                alevel3<
                                alevel4<
                                max_pool<3,3,2,2,relu<affine<con<32,7,7,2,2,
                                input_rgb_image_sized<150>
                                >>>>>>>>>>>>;
    anet_type net;
};

// ----------------------------------------------------------------------------------------

void bind_face_recognition()
{
    using boost::python::arg;
    {
    class_<face_recognition_model_v1>("face_recognition_model_v1", "This object maps human faces into 128D vectors where pictures of the same person are mapped near to each other and pictures of different people are mapped far apart.  The constructor loads the face recognition model from a file. The model file is available here: http://dlib.net/files/dlib_face_recognition_resnet_model_v1.dat.bz2", init<std::string>())
        .def("compute_face_descriptor", &face_recognition_model_v1::compute_face_descriptor, (arg("img"),arg("face"),arg("num_jitters")=0),
            "Takes an image and a full_object_detection that references a face in that image and converts it into a 128D face descriptor. "
            "If num_jitters>1 then each face will be randomly jittered slightly num_jitters times, each run through the 128D projection, and the average used as the face descriptor."
            )
        .def("compute_face_descriptor", &face_recognition_model_v1::compute_face_descriptors, (arg("img"),arg("faces"),arg("num_jitters")=0),
            "Takes an image and an array of full_object_detections that reference faces in that image and converts them into 128D face descriptors.  "
            "If num_jitters>1 then each face will be randomly jittered slightly num_jitters times, each run through the 128D projection, and the average used as the face descriptor."
            );
    }

    {
    typedef std::vector<full_object_detection> type;
    class_<type>("full_object_detections", "An array of full_object_detection objects.")
        .def(vector_indexing_suite<type>())
        .def("clear", &type::clear)
        .def("resize", resize<type>)
        .def_pickle(serialize_pickle<type>());
    }
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/meta/as_logical.hpp>

#if !defined(_MM_PCOMCTRL_LE)
#define _MM_PCOMCTRL_LE 1
#define BOOST_SIMD_MISSING_MM_PCOMCTRL_LE
#endif

#if BOOST_HW_SIMD_X86_AMD_XOP
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          , bs::pack_<bd::int8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi8(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epi8(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          , bs::pack_<bd::int16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi16(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epi16(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi32(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epi32(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epi64(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epi64(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu8(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epu8(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu16(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epu16(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu32(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epu32(a0,a1);
      #endif
    }
  };

  BOOST_DISPATCH_OVERLOAD ( is_less_equal_
                          , (typename A0)
                          , bs::avx_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE
    bs::as_logical_t<A0> operator()(const A0& a0, const A0& a1) const BOOST_NOEXCEPT
    {
      #if defined(__clang__)
      return _mm_com_epu64(a0,a1,_MM_PCOMCTRL_LE);
      #else
      return _mm_comle_epu64(a0,a1);
      #endif
    }
  };
} } }
#endif

#if defined(BOOST_SIMD_MISSING_MM_PCOMCTRL_LE)
#undef _MM_PCOMCTRL_LE
#undef BOOST_SIMD_MISSING_MM_PCOMCTRL_LE
#endif

#endif


//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 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)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_COUNTING_APPLY_HASH_HPP
#define BOOST_BLOOM_FILTER_COUNTING_APPLY_HASH_HPP

#include <boost/mpl/at.hpp>

#include <boost/bloom_filter/detail/exceptions.hpp>

namespace boost {
  namespace bloom_filters {
    namespace detail {

      struct decrement {
	size_t operator()(const size_t val, const size_t limit) {
	  if (val == limit)
	    throw bin_underflow_exception();

	  return val - 1;
	}
      };

      struct increment {
	size_t operator()(const size_t val, const size_t limit) {
	  if (val == limit)
	    throw bin_overflow_exception();

	  return val+1;
	}
      };

      template <size_t N, class CBF, class Op = void>
      struct BloomOp {
	typedef typename boost::mpl::at_c<typename CBF::hash_function_type,
					  N>::type Hash;

      public:
	BloomOp(const typename CBF::value_type& t,
		const typename CBF::bucket_type& slots,
		const size_t num_bins)
	  :
	  hash_val(hasher(t) % num_bins),
	  pos(hash_val / CBF::bins_per_slot()),
	  offset_bits((hash_val % CBF::bins_per_slot()) * CBF::bits_per_bin()),
	  target_bits((slots[pos] >> offset_bits) & CBF::mask())
	{}

	void update(typename CBF::bucket_type& slots,
		    const size_t limit) const {
	  static Op op;

	  const size_t final_bits = op(target_bits, limit);
	  slots[pos] &= ~(CBF::mask() << offset_bits);
	  slots[pos] |= (final_bits << offset_bits);
	}

	bool check() const {
	  return (target_bits != 0);
	}

	Hash hasher;
	const size_t hash_val;
	const size_t pos;
	const size_t offset_bits;
	const size_t target_bits;
      };

      // CBF : Counting Bloom Filter
      template <size_t N,
		class CBF>
      struct counting_apply_hash
      {
	static void insert(const typename CBF::value_type& t,
			   typename CBF::bucket_type& slots,
			   const size_t num_bins)
	{
	  BloomOp<N, CBF, increment> inserter(t, slots, num_bins);
	  inserter.update(slots, (1ull << CBF::bits_per_bin()) - 1ull);

	  counting_apply_hash<N-1, CBF>::insert(t, slots);
	}

	static void remove(const typename CBF::value_type& t,
			   typename CBF::bucket_type& slots,
			   const size_t num_bins)
	{
	  BloomOp<N, CBF, decrement> remover(t, slots, num_bins);
	  remover.update(slots, 0);

	  counting_apply_hash<N-1, CBF>::remove(t, slots);
	}

	static bool contains(const typename CBF::value_type& t,
			     const typename CBF::bucket_type& slots,
			   const size_t num_bins)
	{
	  BloomOp<N, CBF> checker(t, slots, num_bins);
	  return (checker.check() &&
		  counting_apply_hash<N-1, CBF>::contains(t, slots));
	}
      };

      template <class CBF>
      struct counting_apply_hash<0, CBF>
      {
	static void insert(const typename CBF::value_type& t,
			   typename CBF::bucket_type& slots,
			   const size_t num_bins)
	{
	  BloomOp<0, CBF, increment> inserter(t, slots, num_bins);
	  inserter.update(slots, (1ull << CBF::bits_per_bin()) - 1ull);
	}

	static void remove(const typename CBF::value_type& t,
			   typename CBF::bucket_type& slots,
			   const size_t num_bins)
	{
	  BloomOp<0, CBF, decrement> remover(t, slots, num_bins);
	  remover.update(slots, 0);
	}

	static bool contains(const typename CBF::value_type& t,
			     const typename CBF::bucket_type& slots,
			   const size_t num_bins)
	{
	  BloomOp<0, CBF> checker(t, slots, num_bins);
	  return (checker.check());
	}
      };

    } // namespace detail
  } // namespace bloom_filter
} // namespace boost
#endif


#include "CPUGriddingReconGadget.h"
#include "mri_core_grappa.h"
#include "vector_td_utilities.h"
#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include "hoNFFT.h"
#include "hoNDArray.h"
#include "hoNDArray_elemwise.h"
#include "hoNDArray_math.h"
#include "hoCgSolver.h"
#include "ImageArraySendMixin.h"
#include <time.h>
#include <boost/range/algorithm/for_each.hpp>
#include "NonCartesianTools.h"
#include "NFFTOperator.h"
#include "hoNDArray_converter.h"
#include "GriddingReconGadgetBase.hpp"

namespace Gadgetron{

    CPUGriddingReconGadget::CPUGriddingReconGadget() {

    }

    CPUGriddingReconGadget::~CPUGriddingReconGadget() {

    }

    GADGET_FACTORY_DECLARE(CPUGriddingReconGadget);
}


/*
 * TestParticle.cpp
 *
 *  Created on: 26 Feb 2015
 *      Author: kreczko
 */

#include <boost/test/unit_test.hpp>
#include "../interface/RecoObjects/Particle.h"

using namespace BAT;

struct TestParticleSetup {
	TestParticleSetup() :
					particle1(20., 10., 0., 0.),
					particle2(20., -10., 0., 0.),
					zparticle1(100., 79., -13., -5.),
					zparticle2(100., 99., 13., 5.) {
		particle1.setD0(180.);
		particle1.setCharge(0.5);
	}

	~TestParticleSetup() {

	}

	Particle particle1, particle2, zparticle1, zparticle2;
};

BOOST_AUTO_TEST_SUITE (TestParticle)
BOOST_FIXTURE_TEST_CASE(test_get_energy, TestParticleSetup) {
	BOOST_CHECK_EQUAL(particle1.energy(), 20);
}

BOOST_FIXTURE_TEST_CASE(test_get_px, TestParticleSetup) {
	BOOST_CHECK_EQUAL(particle1.px(), 10);
}

BOOST_FIXTURE_TEST_CASE(test_get_py, TestParticleSetup) {
	BOOST_CHECK_EQUAL(particle1.py(), 0);
}

BOOST_FIXTURE_TEST_CASE(test_get_pz, TestParticleSetup) {
	BOOST_CHECK_EQUAL(particle1.pz(), 0);
}

BOOST_FIXTURE_TEST_CASE(test_get_d0, TestParticleSetup) {
	BOOST_CHECK_EQUAL(particle1.d0(), 180.);
}

BOOST_FIXTURE_TEST_CASE(test_get_mass, TestParticleSetup) {
	BOOST_CHECK_CLOSE(particle1.mass(), 17.32, 0.01);
}

BOOST_FIXTURE_TEST_CASE(test_get_charge, TestParticleSetup) {
	BOOST_CHECK_EQUAL(particle1.charge(), 0.5);
}

BOOST_FIXTURE_TEST_CASE(test_operator_plus, TestParticleSetup) {
	Particle p = zparticle1 + zparticle2;
	BOOST_CHECK_EQUAL(p.px(), zparticle1.px() + zparticle2.px());
	BOOST_CHECK_EQUAL(p.py(), zparticle1.py() + zparticle2.py());
	BOOST_CHECK_EQUAL(p.pz(), zparticle1.pz() + zparticle2.pz());
	double e_squared = pow(zparticle1.energy() + zparticle2.energy(), 2);
	double p_squared = pow(zparticle1.px() + zparticle2.px(), 2);
	double mass = sqrt(e_squared - p_squared);
	BOOST_CHECK_EQUAL(p.mass(), mass);
}

BOOST_FIXTURE_TEST_CASE(test_operator_equals, TestParticleSetup) {
	Particle p = particle1;
	BOOST_CHECK_EQUAL(p.px(), particle1.px() );
	BOOST_CHECK_EQUAL(p.py(), particle1.py());
	BOOST_CHECK_EQUAL(p.pz(), particle1.pz());
	BOOST_CHECK_EQUAL(p.energy(), particle1.energy());

	BOOST_CHECK_EQUAL(p.mass(), particle1.mass());
	BOOST_CHECK_EQUAL(p.charge(), particle1.charge());
	BOOST_CHECK_EQUAL(p.d0(), particle1.d0());
}

BOOST_FIXTURE_TEST_CASE(test_operator_plus_equals, TestParticleSetup) {
	Particle p = zparticle1;
	p +=  zparticle2;
	BOOST_CHECK_EQUAL(p.px(), zparticle1.px() + zparticle2.px());
	BOOST_CHECK_EQUAL(p.py(), zparticle1.py() + zparticle2.py());
	BOOST_CHECK_EQUAL(p.pz(), zparticle1.pz() + zparticle2.pz());
	double e_squared = pow(zparticle1.energy() + zparticle2.energy(), 2);
	double p_squared = pow(zparticle1.px() + zparticle2.px(), 2);
	double mass = sqrt(e_squared - p_squared);
	BOOST_CHECK_EQUAL(p.mass(), mass);
}

BOOST_AUTO_TEST_SUITE_END()