#include <cstdint>
#include <stdexcept>

#include <boost/test/unit_test.hpp>

#include "layout/cell_util.hpp"

BOOST_AUTO_TEST_SUITE(cell)

BOOST_AUTO_TEST_CASE(calc_cell_length_and_positions) {
  BOOST_REQUIRE_THROW(
      hisui::layout::calc_cell_length_and_positions({.number_of_cells = 0}),
      std::invalid_argument);
  BOOST_REQUIRE_THROW(
      hisui::layout::calc_cell_length_and_positions(
          {.number_of_cells = 2, .region_length = 240, .min_frame_length = 1}),
      std::invalid_argument);
  {
    auto expected =
        hisui::layout::LengthAndPositions{.length = 236, .positions = {2}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 1,
                                       .region_length = 242,
                                       .min_frame_length = 2}));
  }
  {
    auto expected =
        hisui::layout::LengthAndPositions{.length = 240, .positions = {0}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 1,
                                       .region_length = 242,
                                       .min_frame_length = 2,
                                       .is_frame_on_ends = false}));
  }
  {
    auto expected =
        hisui::layout::LengthAndPositions{.length = 236, .positions = {2}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 1,
                                       .region_length = 240,
                                       .min_frame_length = 2}));
  }
  {
    auto expected =
        hisui::layout::LengthAndPositions{.length = 236, .positions = {2, 240}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 2,
                                       .region_length = 484,
                                       .min_frame_length = 2}));
  }
  {
    auto expected = hisui::layout::LengthAndPositions{
        .length = 156, .positions = {2, 160, 318}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 3,
                                       .region_length = 480,
                                       .min_frame_length = 2}));
  }
  {
    auto expected = hisui::layout::LengthAndPositions{
        .length = 156, .positions = {2, 160, 318}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 3,
                                       .region_length = 480,
                                       .min_frame_length = 2}));
  }
  {
    auto expected = hisui::layout::LengthAndPositions{
        .length = 156, .positions = {0, 158, 316}};
    BOOST_REQUIRE_EQUAL(expected, hisui::layout::calc_cell_length_and_positions(
                                      {.number_of_cells = 3,
                                       .region_length = 480,
                                       .min_frame_length = 2,
                                       .is_frame_on_ends = false}));
  }
}

BOOST_AUTO_TEST_CASE(calc_cell_resolution_and_positions) {
  BOOST_REQUIRE_THROW(hisui::layout::calc_cell_resolution_and_positions(
                          {.grid_dimension = {.columns = 0, .rows = 1}}),
                      std::invalid_argument);
  BOOST_REQUIRE_THROW(hisui::layout::calc_cell_resolution_and_positions(
                          {.grid_dimension = {.columns = 1, .rows = 0}}),
                      std::invalid_argument);

  {
    auto expected = hisui::layout::ResolutionAndPositions{
        .resolution = {.width = 236, .height = 156},
        .positions = {{.x = 2, .y = 2}}};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_cell_resolution_and_positions({
                            .grid_dimension = {.columns = 1, .rows = 1},
                            .region_resolution = {.width = 242, .height = 162},
                        }));
  }
  {
    auto expected = hisui::layout::ResolutionAndPositions{
        .resolution = {.width = 236, .height = 156},
        .positions = {{.x = 2, .y = 2}}};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_cell_resolution_and_positions({
                            .grid_dimension = {.columns = 1, .rows = 1},
                            .region_resolution = {.width = 242, .height = 162},
                            .min_frame_width = 2,
                            .min_frame_height = 2,
                        }));
  }
  {
    auto expected = hisui::layout::ResolutionAndPositions{
        .resolution = {.width = 116, .height = 76},
        .positions = {{.x = 2, .y = 2},
                      {.x = 120, .y = 2},
                      {.x = 2, .y = 80},
                      {.x = 120, .y = 80}}};
    BOOST_REQUIRE_EQUAL(expected,
                        hisui::layout::calc_cell_resolution_and_positions({
                            .grid_dimension = {.columns = 2, .rows = 2},
                            .region_resolution = {.width = 242, .height = 162},
                        }));
  }
}

BOOST_AUTO_TEST_SUITE_END()


#include <sm/eigen/gtest.hpp>
#include <aslam/backend/OptimizerBFGS.hpp>
#include <aslam/backend/OptimizationProblem.hpp>
#include <aslam/backend/ErrorTerm.hpp>
#include <boost/ptr_container/ptr_vector.hpp>
#include <sm/random.hpp>
#include <aslam/backend/test/ErrorTermTester.hpp>
#include <aslam/backend/test/SampleDvAndError.hpp>

TEST(OptimizerBFGSTestSuite, testBFGS)
{
  try {
    using namespace aslam::backend;
    boost::shared_ptr<OptimizationProblem> problem_ptr(new OptimizationProblem);
    OptimizationProblem& problem = *problem_ptr;

    const int P = 2;
    const int E = 3;
    // Add some design variables.
    std::vector< boost::shared_ptr<Point2d> > p2d;
    p2d.reserve(P);
    for (int p = 0; p < P; ++p) {
      boost::shared_ptr<Point2d> point(new Point2d(Eigen::Vector2d::Random())); // random initialization of design variable
      p2d.push_back(point);
      problem.addDesignVariable(point);
      point->setBlockIndex(p);
      point->setActive(true);
    }

    // Add some error terms.
    std::vector< boost::shared_ptr<TestNonSquaredError> > e1;
    e1.reserve(P*E);
    for (int p = 0; p < P; ++p) {
      for (int e = 0; e < E; ++e) {
        TestNonSquaredError::grad_t g(p+1, e+1);
        boost::shared_ptr<TestNonSquaredError> err(new TestNonSquaredError(p2d[p].get(), g));
        err->_p = 1.0;
        e1.push_back(err);
        problem.addErrorTerm(err);
        SCOPED_TRACE("");
        testErrorTerm(err);
      }
    }
    // Now let's optimize.
    OptimizerBFGS::Options options;
    options.maxIterations = 500;
    options.numThreadsJacobian = 8;
    options.convergenceGradientNorm = 0.0;
    options.convergenceDeltaX = 0.0;
    EXPECT_ANY_THROW(options.check());
    options.convergenceGradientNorm = 1e-15;
    EXPECT_NO_THROW(options.check());
    OptimizerBFGS optimizer(options);
    optimizer.setProblem(problem_ptr);

    // Test that linesearch options are correctly forwarded
    options.linesearch.initialStepLength = 1.1;
    optimizer.setOptions(options);
    EXPECT_DOUBLE_EQ(options.linesearch.initialStepLength, optimizer.getLineSearch().options().initialStepLength);

    EXPECT_NO_THROW(optimizer.checkProblemSetup());

    optimizer.initialize();
    SCOPED_TRACE("");
    optimizer.optimize();
    const auto& ret = optimizer.getStatus();

    EXPECT_GT(ret.convergence, ConvergenceStatus::FAILURE);
    EXPECT_LE(ret.gradientNorm, options.convergenceGradientNorm);
    EXPECT_GT(ret.numErrorEvaluations, 0);
    EXPECT_GT(ret.numJacobianEvaluations, 0);
    EXPECT_GE(ret.error, 0.0);
    EXPECT_LT(ret.deltaError, 1e-12);
    EXPECT_LT(ret.maxDeltaX, 1e-3);
    EXPECT_LT(ret.error, std::numeric_limits<double>::max());
    EXPECT_GT(ret.numIterations, 0);

  } catch (const std::exception& e) {
    FAIL() << e.what();
  }
}


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

#pragma once

/// @file
///
/// Code related to for simple nanostructures:
/// nanowires and nanoribbons.
/// This is done using approach developed by Li et. al.
/// see 10.1103/PhysRevB.85.195436 for theoretical
/// expressions. Scaling integrals are solved in
/// non-cartesian coordinates to make them easier
/// to work with

#include <unordered_map>
#include <constants.hpp>
#include <structures.hpp>
#include <qpoint_grid.hpp>
#include <processes.hpp>
#include <isotopic_scattering.hpp>
#include <boost/mpi.hpp>
#if BOOST_VERSION >= 106700
#include <boost/container_hash/hash.hpp>
#else
#include <boost/functional/hash.hpp>
#endif
#include <Eigen/Dense>

/// Hash for pairs and arrays
namespace std {
template <typename T> struct hash<std::pair<T, T>> {
    std::size_t operator()(const std::pair<T, T>& key) const {
        hash<T> backend;
        std::size_t nruter = 0;
        boost::hash_combine(nruter, backend(key.first));
        boost::hash_combine(nruter, backend(key.second));
        return nruter;
    }
};
template <typename T, std::size_t S> struct hash<std::array<T, S>> {
    std::size_t operator()(const array<T, S>& key) const {
        hash<T> backend;
        std::size_t nruter = 0;

        for (auto& e : key)
            boost::hash_combine(nruter, backend(e));
        return nruter;
    }
};
} // namespace std

namespace alma {
namespace nanos {

/// Obtain the scaled lifetime for nanowires (see Eq. 8 of
/// 10.1103/PhysRevB.85.195436) the integral is
/// analitically solved in cilidrical coordinates
/// with z-axis being the transport axis
/// @param[in] tau0 - bulk lifetime
/// @param[in] uaxis - transport axis ( unitary vector )
/// @param[in] vel - phonon velocity
/// @param[in] R - nanowire radii
/// @return scaled tau
double scale_tau_nanowire(double tau0,
                          const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                          const Eigen::Ref<const Eigen::Vector3d>& vel,
                          double R);

/// Obtain the scaled lifetime for nanoribbons(see Eq. 8 of
/// 10.1103/PhysRevB.85.195436) the integral is
/// analitically solved in xy-rotated cartesian
/// coordinates with new y-axis (y') aligned with
/// transport axis. The nanoribbon is then centered
/// and defined from x' = [-L/2,L/2]
/// NOTE: the nanoribbon is contained in xy plane
///       otherwise it will fail.
/// @param[in] tau0 - bulk lifetime
/// @param[in] uaxis - transport axis (unitary vector)
/// @param[in] vel - phonon velocity
/// @param[in] L - nanoribbon width
/// @return scaled tau

double scale_tau_nanoribbon(double tau0,
                            const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                            const Eigen::Ref<const Eigen::Vector3d>& vel,
                            double L);

/// Obtain the thermal conductivity of a nanosystem
/// under RTA
///
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] w - RTA scattering rates
/// @param[in] uaxis - transport axis ( unitary vector )
/// @param[in] system_name - kind of nanosystem
/// @param[in] limiting_length - the limiting length of the nanostructure [nm]
/// @param[in] T - temperature in K
/// @return the RTA thermal conductivity over transpot axis
double calc_kappa_RTA(const alma::Crystal_structure& poscar,
                      const alma::Gamma_grid& grid,
                      const Eigen::Ref<const Eigen::ArrayXXd>& w,
                      const Eigen::Ref<const Eigen::Vector3d>& uaxis,
                      const std::string& system_name,
                      const double limiting_length,
                      double T);

/// Obtain the thermal conductivity of a nanowires system
/// under the full BTE (beyond Relaxation Time Approximation).
/// This is achieved by deterministic solution a linear system
/// over full BZ due to symmetry breaking caused by boundaries
/// (i.e.: scaling of tau can be different for bulk equivalent
///    qpoints. Consequently we cannot use some symmetry operations)
/// @param[in] poscar - description of the unit cell
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] syms - symmetry operations object
/// @param[in] emission_processes   - list of 3-phonon emission processes
/// @param[in] absorption_processes - list of 3-phonon  processes
/// @param[in] isotopic_processes   - list of 2-phonon processes
/// @param[in] w0 - RTA scattering rates
/// @param[in] uaxis - transport axis ( unitary vector )
/// @param[in] system_name - kind of nanosystem
/// @param[in] limiting_length - the limiting length of the nanostructure [nm]
/// @param[in] T - temperature in K
/// @param[in] iterative - use iterative Eigen solver for faster computation
/// @param[in] world - mpi communicator
/// @return the thermal conductivity over transpot axis
double calc_kappa_nanos(
    const alma::Crystal_structure& poscar,
    const alma::Gamma_grid& grid,
    const alma::Symmetry_operations& syms,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        emission_processes,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        absorption_processes,
    std::unordered_map<std::pair<std::size_t, std::size_t>, double>&
        isotopic_processes,
    const Eigen::Ref<const Eigen::ArrayXXd>& w0,
    const Eigen::Ref<const Eigen::Vector3d>& uaxis,
    const std::string& system_name,
    const double limiting_length,
    double T,
    bool iterative,
    boost::mpi::communicator& world);

/// Get processes in full BZ
/// @param[in] grid - phonon spectrum on a regular q-point grid
/// @param[in] cell - description of the unit cell
/// @param[in] syms - symmetry operations object
/// @param[in,out] emission_processes   - list of 3-phonon emission processes
/// @param[in,out] absorption_processes - list of 3-phonon  processes
/// @param[in,out] isotopic_processes   - list of 2-phonon processes
/// @param[in] world - mpi communicator
/// @param[in] scalebroad_three - scale parameter for broadening in 3-ph
/// processes
void get_fullBZ_processes(
    const alma::Gamma_grid& grid,
    const alma::Crystal_structure& cell,
    std::string& anhIFCfile,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        emission_processes,
    std::unordered_map<std::array<std::size_t, 3>, alma::Threeph_process>&
        absorption_processes,
    std::unordered_map<std::pair<std::size_t, std::size_t>, double>&
        isotopic_processes,
    boost::mpi::communicator& world,
    double scalebroad_three = 0.1);

} // namespace nanos
} // namespace alma


#include <boost/numeric/odeint/stepper/controlled_runge_kutta.hpp>


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

#ifndef CRYPTO3_PUBKEY_BLS_CORE_FUNCTIONS_HPP
#define CRYPTO3_PUBKEY_BLS_CORE_FUNCTIONS_HPP

#include <utility>
#include <vector>
#include <array>
#include <type_traits>
#include <iterator>
#include <algorithm>

#include <boost/assert.hpp>
#include <boost/concept_check.hpp>

#include <boost/range/concepts.hpp>

#include <nil/crypto3/hash/algorithm/to_curve.hpp>

#include <nil/crypto3/algebra/curves/bls12.hpp>

#include <nil/crypto3/detail/type_traits.hpp>

namespace nil {
    namespace crypto3 {
        namespace pubkey {
            namespace detail {
                template<typename policy_type>
                struct bls_basic_functions {
                    typedef typename policy_type::curve_type curve_type;
                    typedef typename policy_type::gt_value_type gt_value_type;
                    typedef typename policy_type::private_key_type private_key_type;
                    typedef typename policy_type::public_key_type public_key_type;
                    typedef typename policy_type::signature_type signature_type;
                    typedef typename policy_type::h2c_policy h2c_policy;

                    typedef typename policy_type::bls_serializer bls_serializer;
                    typedef typename policy_type::public_key_serialized_type public_key_serialized_type;
                    typedef typename policy_type::signature_serialized_type signature_serialized_type;

                    typedef typename policy_type::internal_accumulator_type internal_accumulator_type;
                    typedef std::pair<std::vector<public_key_type>, std::vector<internal_accumulator_type>>
                        internal_aggregation_accumulator_type;
                    typedef std::pair<std::vector<public_key_type>, internal_accumulator_type>
                        internal_fast_aggregation_accumulator_type;

                    constexpr static const std::size_t private_key_bits = policy_type::private_key_bits;
                    constexpr static const std::size_t L = static_cast<std::size_t>((3 * private_key_bits) / 16) +
                                                           static_cast<std::size_t>((3 * private_key_bits) % 16 != 0);
                    static_assert(L < 0x10000, "L is required to fit in 2 octets");
                    constexpr static const std::array<std::uint8_t, 2> L_os = {static_cast<std::uint8_t>(L >> 8u),
                                                                               static_cast<std::uint8_t>(L % 0x100)};

                    // TODO: implement key_gen
                    // template<typename IkmType, typename KeyInfoType>
                    // static inline private_key_type key_gen(const IkmType &ikm, const KeyInfoType &key_info) {}

                    static inline bool validate_private_key(const private_key_type &sk) {
                        return !sk.is_zero();
                    }

                    static inline public_key_type privkey_to_pubkey(const private_key_type &sk) {
                        BOOST_ASSERT(validate_private_key(sk));

                        return sk * public_key_type::one();
                    }

                    static inline bool validate_public_key(const public_key_type &pk) {
                        return !(pk.is_zero() || !pk.is_well_formed());
                    }

                    template<typename InputRange>
                    static inline void update(internal_accumulator_type &acc, const InputRange &range) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<InputRange>));

                        to_curve<h2c_policy>(range, acc);
                    }

                    template<typename InputIterator>
                    static inline void update(internal_accumulator_type &acc, InputIterator first, InputIterator last) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<InputIterator>));

                        to_curve<h2c_policy>(first, last, acc);
                    }

                    static inline signature_type sign(const internal_accumulator_type &acc,
                                                      const private_key_type &sk) {
                        BOOST_ASSERT(validate_private_key(sk));

                        signature_type Q = hashes::accumulators::extract::to_curve<h2c_policy>(acc);
                        return sk * Q;
                    }

                    static inline bool verify(const internal_accumulator_type &acc, const public_key_type &pk,
                                              const signature_type &sig) {
                        /// check if signature point is on the curve
                        if (!sig.is_well_formed()) {
                            return false;
                        }
                        if (!validate_public_key(pk)) {
                            return false;
                        }
                        signature_type Q = hashes::accumulators::extract::to_curve<h2c_policy>(acc);
                        auto C1 = policy_type::pairing(Q, pk);
                        auto C2 = policy_type::pairing(sig, public_key_type::one());
                        return C1 == C2;
                    }

                    template<
                        typename SignatureIterator,
                        typename = typename std::enable_if<std::is_same<
                            signature_type, typename std::iterator_traits<SignatureIterator>::value_type>::value>::type>
                    static inline void aggregate(signature_type &acc, SignatureIterator sig_first,
                                                 SignatureIterator sig_last) {
                        BOOST_CONCEPT_ASSERT((boost::InputIteratorConcept<SignatureIterator>));
                        assert(std::distance(sig_first, sig_last) > 0);

                        while (sig_first != sig_last) {
                            signature_type next_p = *sig_first++;
                            acc = acc + next_p;
                        }
                    }

                    template<typename SignatureRange,
                             typename = typename std::enable_if<std::is_same<
                                 signature_type, typename std::iterator_traits<
                                                     typename SignatureRange::iterator>::value_type>::value>::type>
                    static inline void aggregate(signature_type &acc, const SignatureRange &sig_n) {
                        BOOST_CONCEPT_ASSERT((boost::SinglePassRangeConcept<SignatureRange>));

                        aggregate(acc, std::cbegin(sig_n), std::cend(sig_n));
                    }

                    static inline bool aggregate_verify(const internal_aggregation_accumulator_type &acc,
                                                        const signature_type &sig) {
                        const typename internal_aggregation_accumulator_type::first_type &pk_n = acc.first;
                        const typename internal_aggregation_accumulator_type::second_type &acc_n = acc.second;
                        assert(std::distance(pk_n.begin(), pk_n.end()) > 0 &&
                               std::distance(pk_n.begin(), pk_n.end()) == std::distance(acc_n.begin(), acc_n.end()));

                        if (!sig.is_well_formed()) {
                            return false;
                        }
                        auto pk_n_iter = std::cbegin(pk_n);
                        auto acc_n_iter = std::cbegin(acc_n);
                        gt_value_type C1 = gt_value_type::one();
                        while (pk_n_iter != std::cend(pk_n) && acc_n_iter != std::cend(acc_n)) {
                            if (!validate_public_key(*pk_n_iter)) {
                                return false;
                            }
                            signature_type Q = hashes::accumulators::extract::to_curve<h2c_policy>(*acc_n_iter++);
                            C1 = C1 * policy_type::pairing(Q, *pk_n_iter++);
                        }
                        return C1 == policy_type::pairing(sig, public_key_type::one());
                    }

                    static inline bool aggregate_verify(const internal_fast_aggregation_accumulator_type &acc,
                                                        const signature_type &sig) {
                        const typename internal_fast_aggregation_accumulator_type::first_type &pk_n = acc.first;
                        const typename internal_fast_aggregation_accumulator_type::second_type &msg_acc = acc.second;
                        assert(std::distance(pk_n.begin(), pk_n.end()) > 0);

                        auto pk_n_iter = pk_n.begin();
                        public_key_type aggregate_p = *pk_n_iter++;
                        while (pk_n_iter != pk_n.end()) {
                            public_key_type next_p = *pk_n_iter++;
                            aggregate_p = aggregate_p + next_p;
                        }
                        return verify(msg_acc, aggregate_p, sig);
                    }

                    static inline signature_type pop_prove(const private_key_type &sk) {
                        assert(validate_private_key(sk));

                        public_key_type pk = privkey_to_pubkey(sk);
                        signature_type Q = to_curve<h2c_policy>(point_to_pubkey(pk));
                        return sk * Q;
                    }

                    static inline bool pop_verify(const public_key_type &pk, const signature_type &pop) {
                        if (!pop.is_well_formed()) {
                            return false;
                        }
                        if (!validate_public_key(pk)) {
                            return false;
                        }
                        signature_type Q = to_curve<h2c_policy>(point_to_pubkey(pk));
                        auto C1 = policy_type::pairing(Q, pk);
                        auto C2 = policy_type::pairing(pop, public_key_type::one());
                        return C1 == C2;
                    }

                    static inline public_key_serialized_type point_to_pubkey(const public_key_type &pk) {
                        return bls_serializer::point_to_octets_compress(pk);
                    }

                    static inline signature_serialized_type point_to_signature(const signature_type &sig) {
                        return bls_serializer::point_to_octets_compress(sig);
                    }
                };
            }    // namespace detail
        }        // namespace pubkey
    }            // namespace crypto3
}    // namespace nil

#endif    // CRYPTO3_PUBKEY_BLS_CORE_FUNCTIONS_HPP


//////////////////////////////////////////////////////////////////////////////////////
// distribution::toolkit::distributions::::distributions::exponential::include.hpp   //
//                                                                                //
//  (C) Copyright 2009 Erwann Rogard                                                //
//  Use, modification and distribution are 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_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_EXPONENTIAL_INCLUDE_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_DISTRIBUTION_TOOLKIT_EXPONENTIAL_INCLUDE_HPP_ER_2009

#include <boost/math/distributions/exponential.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/description.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/derivative_log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/is_math_distribution.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/log_unnormalized_pdf.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/random.hpp>
#include <boost/statistics/detail/distribution_toolkit/distributions/exponential/os.hpp>

#endif


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

#include <dlib/python.h>
#include "testing_results.h"
#include <boost/shared_ptr.hpp>
#include <dlib/matrix.h>
#include <dlib/svm_threaded.h>
#include <boost/python/args.hpp>

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

typedef matrix<double,0,1> sample_type;
typedef std::vector<std::pair<unsigned long,double> > sparse_vect;

template <typename trainer_type>
typename trainer_type::trained_function_type train (
    const trainer_type& trainer,
    const std::vector<typename trainer_type::sample_type>& samples,
    const std::vector<double>& labels
)
{
    pyassert(is_binary_classification_problem(samples,labels), "Invalid inputs");
    return trainer.train(samples, labels);
}

template <typename trainer_type>
void set_epsilon ( trainer_type& trainer, double eps)
{
    pyassert(eps > 0, "epsilon must be > 0");
    trainer.set_epsilon(eps);
}

template <typename trainer_type>
double get_epsilon ( const trainer_type& trainer) { return trainer.get_epsilon(); }

template <typename trainer_type>
void set_cache_size ( trainer_type& trainer, long cache_size)
{
    pyassert(cache_size > 0, "cache size must be > 0");
    trainer.set_cache_size(cache_size);
}

template <typename trainer_type>
long get_cache_size ( const trainer_type& trainer) { return trainer.get_cache_size(); }

template <typename trainer_type>
void set_c ( trainer_type& trainer, double C)
{
    pyassert(C > 0, "C must be > 0");
    trainer.set_c(C);
}

template <typename trainer_type>
void set_c_class1 ( trainer_type& trainer, double C)
{
    pyassert(C > 0, "C must be > 0");
    trainer.set_c_class1(C);
}

template <typename trainer_type>
void set_c_class2 ( trainer_type& trainer, double C)
{
    pyassert(C > 0, "C must be > 0");
    trainer.set_c_class2(C);
}

template <typename trainer_type>
double get_c_class1 ( const trainer_type& trainer) { return trainer.get_c_class1(); }
template <typename trainer_type>
double get_c_class2 ( const trainer_type& trainer) { return trainer.get_c_class2(); }

template <typename trainer_type>
class_<trainer_type> setup_trainer (
    const std::string& name
)
{
    return class_<trainer_type>(name.c_str())
        .def("train", train<trainer_type>)
        .def("set_c", set_c<trainer_type>)
        .add_property("c_class1", get_c_class1<trainer_type>, set_c_class1<trainer_type>)
        .add_property("c_class2", get_c_class2<trainer_type>, set_c_class2<trainer_type>)
        .add_property("epsilon", get_epsilon<trainer_type>, set_epsilon<trainer_type>);
}

template <typename trainer_type>
class_<trainer_type> setup_trainer2 (
    const std::string& name
)
{

    return setup_trainer<trainer_type>(name)
        .add_property("cache_size", get_cache_size<trainer_type>, set_cache_size<trainer_type>);
}

void set_gamma (
    svm_c_trainer<radial_basis_kernel<sample_type> >& trainer,
    double gamma
)
{
    pyassert(gamma > 0, "gamma must be > 0");
    trainer.set_kernel(radial_basis_kernel<sample_type>(gamma));
}

double get_gamma (
    const svm_c_trainer<radial_basis_kernel<sample_type> >& trainer
)
{
    return trainer.get_kernel().gamma;
}

void set_gamma_sparse (
    svm_c_trainer<sparse_radial_basis_kernel<sparse_vect> >& trainer,
    double gamma
)
{
    pyassert(gamma > 0, "gamma must be > 0");
    trainer.set_kernel(sparse_radial_basis_kernel<sparse_vect>(gamma));
}

double get_gamma_sparse (
    const svm_c_trainer<sparse_radial_basis_kernel<sparse_vect> >& trainer
)
{
    return trainer.get_kernel().gamma;
}

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

template <
    typename trainer_type
    >
const binary_test _cross_validate_trainer (
    const trainer_type& trainer,
    const std::vector<typename trainer_type::sample_type>& x,
    const std::vector<double>& y,
    const unsigned long folds
)
{
    pyassert(is_binary_classification_problem(x,y), "Training data does not make a valid training set.");
    pyassert(1 < folds && folds <= x.size(), "Invalid number of folds given.");
    return cross_validate_trainer(trainer, x, y, folds);
}

template <
    typename trainer_type
    >
const binary_test _cross_validate_trainer_t (
    const trainer_type& trainer,
    const std::vector<typename trainer_type::sample_type>& x,
    const std::vector<double>& y,
    const unsigned long folds,
    const unsigned long num_threads
)
{
    pyassert(is_binary_classification_problem(x,y), "Training data does not make a valid training set.");
    pyassert(1 < folds && folds <= x.size(), "Invalid number of folds given.");
    pyassert(1 < num_threads, "The number of threads specified must not be zero.");
    return cross_validate_trainer_threaded(trainer, x, y, folds, num_threads);
}

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

void bind_svm_c_trainer()
{
    using boost::python::arg;
    {
        typedef svm_c_trainer<radial_basis_kernel<sample_type> > T;
        setup_trainer2<T>("svm_c_trainer_radial_basis")
            .add_property("gamma", get_gamma, set_gamma);
        def("cross_validate_trainer", _cross_validate_trainer<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds")));
        def("cross_validate_trainer_threaded", _cross_validate_trainer_t<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds"),arg("num_threads")));
    }

    {
        typedef svm_c_trainer<sparse_radial_basis_kernel<sparse_vect> > T;
        setup_trainer2<T>("svm_c_trainer_sparse_radial_basis")
            .add_property("gamma", get_gamma_sparse, set_gamma_sparse);
        def("cross_validate_trainer", _cross_validate_trainer<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds")));
        def("cross_validate_trainer_threaded", _cross_validate_trainer_t<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds"),arg("num_threads")));
    }

    {
        typedef svm_c_trainer<histogram_intersection_kernel<sample_type> > T;
        setup_trainer2<T>("svm_c_trainer_histogram_intersection");
        def("cross_validate_trainer", _cross_validate_trainer<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds")));
        def("cross_validate_trainer_threaded", _cross_validate_trainer_t<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds"),arg("num_threads")));
    }

    {
        typedef svm_c_trainer<sparse_histogram_intersection_kernel<sparse_vect> > T;
        setup_trainer2<T>("svm_c_trainer_sparse_histogram_intersection");
        def("cross_validate_trainer", _cross_validate_trainer<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds")));
        def("cross_validate_trainer_threaded", _cross_validate_trainer_t<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds"),arg("num_threads")));
    }

    {
        typedef svm_c_linear_trainer<linear_kernel<sample_type> > T;
        setup_trainer<T>("svm_c_trainer_linear")
            .add_property("max_iterations", &T::get_max_iterations, &T::set_max_iterations)
            .add_property("force_last_weight_to_1", &T::forces_last_weight_to_1, &T::force_last_weight_to_1)
            .add_property("learns_nonnegative_weights", &T::learns_nonnegative_weights, &T::set_learns_nonnegative_weights)
            .add_property("has_prior", &T::has_prior)
            .def("set_prior", &T::set_prior)
            .def("be_verbose", &T::be_verbose)
            .def("be_quiet", &T::be_quiet);

        def("cross_validate_trainer", _cross_validate_trainer<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds")));
        def("cross_validate_trainer_threaded", _cross_validate_trainer_t<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds"),arg("num_threads")));
    }

    {
        typedef svm_c_linear_trainer<sparse_linear_kernel<sparse_vect> > T;
        setup_trainer<T>("svm_c_trainer_sparse_linear")
            .add_property("max_iterations", &T::get_max_iterations, &T::set_max_iterations)
            .add_property("force_last_weight_to_1", &T::forces_last_weight_to_1, &T::force_last_weight_to_1)
            .add_property("learns_nonnegative_weights", &T::learns_nonnegative_weights, &T::set_learns_nonnegative_weights)
            .add_property("has_prior", &T::has_prior)
            .def("set_prior", &T::set_prior)
            .def("be_verbose", &T::be_verbose)
            .def("be_quiet", &T::be_quiet);

        def("cross_validate_trainer", _cross_validate_trainer<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds")));
        def("cross_validate_trainer_threaded", _cross_validate_trainer_t<T>,
            (arg("trainer"),arg("x"),arg("y"),arg("folds"),arg("num_threads")));
    }
}


#ifndef STAN_MATH_OPENCL_PRIM_CATEGORICAL_LOGIT_GLM_LPMF_HPP
#define STAN_MATH_OPENCL_PRIM_CATEGORICAL_LOGIT_GLM_LPMF_HPP
#ifdef STAN_OPENCL

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/size_zero.hpp>
#include <stan/math/prim/fun/sum.hpp>
#include <Eigen/Core>

#include <stan/math/opencl/matrix_cl.hpp>
#include <stan/math/opencl/copy.hpp>
#include <stan/math/opencl/multiply.hpp>
#include <stan/math/opencl/kernel_generator.hpp>
#include <stan/math/opencl/kernels/categorical_logit_glm_lpmf.hpp>

namespace stan {
namespace math {

/** \ingroup opencl
 * Returns the log PMF of the Generalized Linear Model (GLM)
 * with categorical distribution and logit (softmax) link function.
 * This is an overload of the GLM in
 * prim/prob/categorical_logit_glm_lpmf.hpp that is implemented in OpenCL.
 *
 * @tparam T_alpha_scalar type of scalar in the intercept vector
 * @tparam T_beta_scalar type of a scalar in the matrix of weights
 * @param y_cl a scalar or vector of classes. If it is a scalar it will be
 * broadcast - used for all instances. Values should be between 1 and number of
 * classes, including endpoints.
 * @param x_cl design matrix on OpenCL device. This overload does not support
 * broadcasting of a row vector x!
 * @param alpha intercept vector (in log odds)
 * @param beta weight matrix
 * @return log probability or log sum of probabilities
 * @throw std::domain_error x, beta or alpha is infinite or y is not within
 * bounds
 * @throw std::invalid_argument if container sizes mismatch.
 */
template <bool propto, typename T_alpha_scalar, typename T_beta_scalar>
return_type_t<T_alpha_scalar, T_beta_scalar> categorical_logit_glm_lpmf(
    const matrix_cl<int>& y_cl, const matrix_cl<double>& x_cl,
    const Eigen::Matrix<T_alpha_scalar, Eigen::Dynamic, 1>& alpha,
    const Eigen::Matrix<T_beta_scalar, Eigen::Dynamic, Eigen::Dynamic>& beta) {
  using T_partials_return = partials_return_t<T_alpha_scalar, T_beta_scalar>;
  static const char* function = "categorical_logit_glm_lpmf";

  using Eigen::Array;
  using Eigen::Dynamic;
  using Eigen::Matrix;

  const size_t N_instances = x_cl.rows();
  const size_t N_attributes = x_cl.cols();
  const size_t N_classes = beta.cols();

  if (y_cl.size() != 1) {
    check_size_match(function, "x.rows()", N_instances, "y.size()",
                     y_cl.size());
  }
  check_consistent_size(function, "Intercept vector", alpha, N_classes);
  check_size_match(function, "x.cols()", N_attributes, "beta.rows()",
                   beta.rows());

  if (N_instances == 0 || N_classes <= 1) {
    return 0;
  }

  if (!include_summand<propto, T_alpha_scalar, T_beta_scalar>::value) {
    return 0;
  }

  const auto& beta_val = value_of_rec(beta);
  const auto& alpha_val = value_of_rec(alpha);

  const auto& alpha_val_vec = as_column_vector_or_scalar(alpha_val).transpose();

  const int local_size
      = opencl_kernels::categorical_logit_glm.get_option("LOCAL_SIZE_");
  const int wgs = (N_instances + local_size - 1) / local_size;

  const matrix_cl<double> beta_cl(beta_val);
  const matrix_cl<double> alpha_cl(alpha_val);

  matrix_cl<double> x_beta_cl = x_cl * beta_cl;

  bool need_alpha_derivative = !is_constant_all<T_alpha_scalar>::value;
  bool need_beta_derivative = !is_constant_all<T_beta_scalar>::value;

  matrix_cl<double> logp_cl(wgs, 1);
  matrix_cl<double> exp_lin_cl(N_instances, N_classes);
  matrix_cl<double> inv_sum_exp_lin_cl(N_instances, 1);
  matrix_cl<double> neg_softmax_lin_cl(
      need_alpha_derivative || need_beta_derivative ? N_instances : 0,
      N_classes);
  matrix_cl<double> alpha_derivative_cl(need_alpha_derivative ? wgs : 0,
                                        N_classes);

  try {
    opencl_kernels::categorical_logit_glm(
        cl::NDRange(local_size * wgs), cl::NDRange(local_size), logp_cl,
        exp_lin_cl, inv_sum_exp_lin_cl, neg_softmax_lin_cl, alpha_derivative_cl,
        y_cl, x_beta_cl, alpha_cl, N_instances, N_attributes, N_classes,
        y_cl.size() != 1, need_alpha_derivative, need_beta_derivative);
  } catch (const cl::Error& e) {
    check_opencl_error(function, e);
  }
  T_partials_return logp = sum(from_matrix_cl(logp_cl));

  if (!std::isfinite(logp)) {
    check_bounded(function, "categorical outcome out of support",
                  from_matrix_cl(y_cl), 1, N_classes);
    check_finite(function, "Weight vector", beta);
    check_finite(function, "Intercept", alpha);
    check_finite(function, "Matrix of independent variables",
                 from_matrix_cl(x_cl));
  }

  operands_and_partials<Matrix<T_alpha_scalar, Dynamic, 1>,
                        Matrix<T_beta_scalar, Dynamic, Dynamic>>
      ops_partials(alpha, beta);
  if (!is_constant_all<T_alpha_scalar>::value) {
    ops_partials.edge1_.partials_
        = from_matrix_cl(alpha_derivative_cl).colwise().sum();
  }
  if (!is_constant_all<T_beta_scalar>::value && N_attributes != 0) {
    matrix_cl<double> beta_derivative_cl = transpose(x_cl) * neg_softmax_lin_cl;
    matrix_cl<double> temp(N_classes, local_size * N_attributes);
    try {
      opencl_kernels::categorical_logit_glm_beta_derivative(
          cl::NDRange(local_size * N_attributes), cl::NDRange(local_size),
          beta_derivative_cl, temp, y_cl, x_cl, N_instances, N_attributes,
          N_classes, y_cl.size() != 1);
    } catch (const cl::Error& e) {
      check_opencl_error(function, e);
    }
    ops_partials.edge2_.partials_ = from_matrix_cl(beta_derivative_cl);
  }
  return ops_partials.build(logp);
}

template <typename T_alpha_scalar, typename T_beta_scalar>
return_type_t<T_alpha_scalar, T_beta_scalar> categorical_logit_glm_lpmf(
    const matrix_cl<int>& y, const matrix_cl<double>& x,
    const Eigen::Matrix<T_alpha_scalar, Eigen::Dynamic, 1>& alpha,
    const Eigen::Matrix<T_beta_scalar, Eigen::Dynamic, Eigen::Dynamic>& beta) {
  return categorical_logit_glm_lpmf<false>(y, x, alpha, beta);
}

}  // namespace math
}  // namespace stan

#endif
#endif


#include <rovi_pose_estimator/rovi_pose_estimator.h>

#include <iostream>
#include <vector>
#include <tuple>

#include <ros/ros.h>
#include <ros/package.h>

#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <std_msgs/Int32.h>

#include <Eigen/Eigen>
#include <eigen_conversions/eigen_msg.h>

#include <opencv4/opencv2/imgproc.hpp>
#include <opencv4/opencv2/core.hpp>
#include <opencv4/opencv2/imgcodecs.hpp>
#include <opencv4/opencv2/highgui.hpp>
#include <opencv4/opencv2/flann.hpp>
#include <opencv4/opencv2/calib3d.hpp>
#include <opencv4/opencv2/stereo.hpp>
#include <opencv4/opencv2/ximgproc/disparity_filter.hpp>
#include <opencv4/opencv2/core/eigen.hpp>
#include <cv_bridge/cv_bridge.h>

#include <pcl/features/normal_3d.h>
#include <pcl/features/spin_image.h>

#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/crop_box.h>
#include <pcl/filters/passthrough.h>

#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/visualization/pcl_visualizer.h>

#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>

#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/transformation_estimation_svd.h>
#include <pcl/registration/icp.h>

// #include <pcl/surface/mls.h>
// #include <pcl/surface/impl/mls.hpp>
// #include <pcl/surface/bilateral_upsampling.h>

#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>

#include <pcl/common/random.h>
#include <pcl/common/time.h>
#include <pcl/common/transforms.h>

#include <rovi_gazebo/rovi_gazebo.h>
#include <rovi_utils/rovi_utils.h>

namespace rovi_pose_estimator
{

// Scene
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_scene_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_orig_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filtered_scene_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::Histogram<153>>::Ptr features_scene_ptr(new pcl::PointCloud<pcl::Histogram<153>>());

// Object
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud_object_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr filtered_object_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());
static pcl::PointCloud<pcl::Histogram<153>>::Ptr features_object_ptr(new pcl::PointCloud<pcl::Histogram<153>>());

// Correspondences
static pcl::Correspondences corr;

std::array<cv::Mat, 2>
M1::get_image_data(const std::string & ns_ros)
{
    static std::mutex mutex_image_l;
    static sensor_msgs::Image image_l;
    static auto thread_left = rovi_utils::create_async_listener(ns_ros + "/left/image_raw", image_l, mutex_image_l);

    static std::mutex mutex_image_r;
    static sensor_msgs::Image image_r;
    static auto thread_right = rovi_utils::create_async_listener(ns_ros + "/right/image_raw", image_r, mutex_image_r);

    static bool first = true;

    std::array<cv::Mat, 2> arr;
    cv::Mat temp_l, temp_r;

    std::lock_guard<std::mutex> lock_l(mutex_image_l);
    std::lock_guard<std::mutex> lock_r(mutex_image_r);

    auto ptr_image_l = boost::make_shared<const sensor_msgs::Image>(image_l);
    auto ptr_image_r = boost::make_shared<const sensor_msgs::Image>(image_r);

    cv_bridge::toCvShare(ptr_image_l, "bgr8")->image.copyTo(temp_l);
    arr[0] = temp_l;
    cv_bridge::toCvShare(ptr_image_r, "bgr8")->image.copyTo(temp_r);
    arr[1] = temp_r;

    return arr;
}

void
M1::set_structed_light(ros::NodeHandle & nh, const bool & state)
{
    static auto handler = nh.advertise<std_msgs::Int32>("/projector_controller/projector", 1);

    // spam the buffer
    ros::Rate lp(200);
    auto tic = ros::Time::now();
    while (1.0 > (ros::Time::now() - tic).toSec())
    {
        // ROS_INFO_STREAM((ros::Time::now() - tic).toSec());
        std_msgs::Int32 msg;
        msg.data = ( state == 1 ) ? 1 : 0;
        handler.publish(msg);
        lp.sleep();
    }
}

std::array<sensor_msgs::CameraInfo, 2>
M1::get_image_info(const std::string & ns_ros)
{
    std::array<sensor_msgs::CameraInfo, 2> arr;

    for (const auto & [idx, cam] : std::array{std::tuple(0, std::string("/left/camera_info")), std::tuple(1, std::string("/right/camera_info"))})
    {
        // get the message
        auto msg = ros::topic::waitForMessage<sensor_msgs::CameraInfo>(ns_ros + cam);
        arr[idx] = *msg;
    }
    return arr;
}

cv::Mat
M1::compute_disparitymap(const cv::Mat & img_left, const cv::Mat & img_right)
{
    // FileStorage
    cv::FileStorage fs("stereosgbm_config.yaml", cv::FileStorage::READ);

    static auto min_disp          = 0;
    static auto num_disp          = 16*8;
    static auto block_size        = 5; // has to be odd
    static auto cn                = 3;
    static auto p1_smoothness     = 100;
    static auto p2_smoothness     = 400; // he larger the values are, the smoother the disparity is
    static auto disp_12_max       = 1;
    static auto unique_ratio      = 5;
    static auto speckle_win_size  = 0;
    static auto specke_range      = 1;
    static auto filter_gap        = 2;
    static auto written           = 0;
    static auto lambda            = 2;
    static auto sigma             = 5;

    // // Check if the file exists
    fs["written"] >> written;
    // ROS_INFO_STREAM_ONCE("Checking if the file exists stereosgbm_config.yaml exists, the state is: " << written);
    // ROS_INFO_STREAM_ONCE("If not, write to the file");

    if (fs.isOpened() && written == 1)
    {
        //read from file
        fs["min_disp"]         >> min_disp;
        fs["num_disp"]         >> num_disp;
        fs["block_size"]       >> block_size;
        fs["cn"]               >> cn;
        fs["p1_smoothness"]    >> p1_smoothness;
        fs["p2_smoothness"]    >> p2_smoothness;
        fs["disp_12_max"]      >> disp_12_max;
        fs["unique_ratio"]     >> unique_ratio;
        fs["speckle_win_size"] >> speckle_win_size;
        fs["specke_range"]     >> specke_range;
        fs["filter_gap"]       >> filter_gap;
        fs["lambda"]           >> lambda;
        fs["sigma"]            >> sigma;
    }
    else
    {
        // write to file
        cv::FileStorage fs1("stereosgbm_config.yaml", cv::FileStorage::WRITE);
        fs1 << "written"          << 1;
        fs1 << "min_disp"         << min_disp;
        fs1 << "num_disp"         << num_disp;
        fs1 << "block_size"       << block_size;
        fs1 << "cn"               << cn;
        fs1 << "p1_smoothness"    << p1_smoothness;
        fs1 << "p2_smoothness"    << p2_smoothness;
        fs1 << "disp_12_max"      << disp_12_max;
        fs1 << "unique_ratio"     << unique_ratio;
        fs1 << "speckle_win_size" << speckle_win_size;
        fs1 << "specke_range"     << specke_range;
        fs1 << "filter_gap"       << filter_gap;
        fs1 << "sigma"            << sigma;
        fs1 << "lambda"           << lambda;
    }

    // Write the configurations
    ROS_INFO_STREAM( min_disp           << ", " <<
                     num_disp           << ", " <<
                     block_size         << ", " <<
                     cn                 << ", " <<
                     p1_smoothness      << ", " <<
                     p2_smoothness      << ", " <<
                     disp_12_max        << ", " <<
                     unique_ratio       << ", " <<
                     speckle_win_size   << ", " <<
                     specke_range       << ", " <<
                     filter_gap
                    );

    // Setup the matchers
    cv::Ptr<cv::StereoSGBM> left_matcher = cv::StereoSGBM::create(
                                                                min_disp,
                                                                num_disp,
                                                                block_size,
                                                                p1_smoothness,
                                                                p2_smoothness,
                                                                disp_12_max,
                                                                filter_gap,
                                                                unique_ratio,
                                                                speckle_win_size,
                                                                specke_range,
                                                                cv::StereoSGBM::MODE_SGBM_3WAY
                                                              );

    // Use the recomended solvers from OpenCV
    cv::Ptr<cv::ximgproc::DisparityWLSFilter> wls = cv::ximgproc::createDisparityWLSFilter(left_matcher);
    cv::Ptr<cv::StereoMatcher> right_matcher = cv::ximgproc::createRightMatcher(left_matcher);

    // For plotting the sets, define 5 cv::Mat
    cv::Mat disp_left, disp_right, disp_filtered, img_left_temp, img_right_temp, point_cloud;

    // Color convert
    cv::cvtColor(img_left,  img_left_temp, cv::COLOR_BGR2GRAY);
    cv::cvtColor(img_right, img_right_temp, cv::COLOR_BGR2GRAY);

    // Remove noise before canny, always.
    cv::GaussianBlur(img_left_temp, img_left_temp, GAUSS_BLUR_KERNEL, GAUSS_BLUR_STD);
    cv::GaussianBlur(img_right_temp, img_right_temp, GAUSS_BLUR_KERNEL, GAUSS_BLUR_STD);

    // Compute left and right disparity
    left_matcher->compute(img_left_temp, img_right_temp, disp_left);
    right_matcher->compute(img_right_temp, img_left_temp, disp_right);

    // Computed weighted least squares optimization
    wls->setLambda(lambda);
    wls->setSigmaColor(sigma);
    wls->filter(disp_left, img_left, disp_filtered, disp_right);

    // Compute Disparity Map
    cv::ximgproc::getDisparityVis(disp_filtered, disp_filtered, 1.0);
    // cv::imwrite("disparity_map.jpg", disp_filtered);

    // Define the Camera matrix
    static auto cam_info_arr = M1::get_image_info();
    static Eigen::Matrix<double, 3, 3, Eigen::RowMajor> K_left(cam_info_arr[LEFT].K.data());

    // Compute the Q-matrix
    static cv::Mat Q = (cv::Mat_<double>(4, 4)<< 1.f, 0.f,  0.f, -K_left(0,2),
                                                 0.f, 1.f,  0.f, -K_left(1,2),
                                                 0.f, 0.f,  0.f,  K_left(1,1),
                                                 0.f, 0.f, -1.f/BASELINE, 0.f );

    // Reproject the image to 3D
    cv::reprojectImageTo3D(disp_filtered, point_cloud, Q, false);

    return point_cloud;
};

void
M1::compute_pointcloud_scene(const cv::Mat & point_cloud, const cv::Mat & left_img)
{

    // cloud_ptr set width etc
    cloud_scene_ptr->width = point_cloud.cols;
    cloud_scene_ptr->height = point_cloud.rows;
    cloud_scene_ptr->is_dense = false;
    cloud_scene_ptr->resize(cloud_scene_ptr->width * cloud_scene_ptr->height);

    // these are reuqired for the for loop
    for (int i = 0, m = 0, k = 0; i < point_cloud.rows; ++i)
    {
        const float* point_cloud_ele = point_cloud.ptr<float>(i);
        const uchar* rbg_left = left_img.ptr<uchar>(i);
        m = 0;

        for(int j = 0; j < point_cloud.cols * 3; )
        {

            std::uint8_t b = (std::uint8_t) rbg_left[m++];
            std::uint8_t g = (std::uint8_t) rbg_left[m++];
            std::uint8_t r = (std::uint8_t) rbg_left[m++];

            cloud_scene_ptr -> points[k].x = point_cloud_ele[j++];
            cloud_scene_ptr -> points[k].y = point_cloud_ele[j++];
            cloud_scene_ptr -> points[k].z = point_cloud_ele[j++];

            std::uint32_t rgb = ( (uint32_t) r << 24 | (uint32_t) g << 16 | (uint32_t) b << 8);

            cloud_scene_ptr -> points[k++].rgb = *reinterpret_cast<float*>(&rgb);
        }
    }

    // Compute the transformation matrix from world to gazebo camera
    static Eigen::Affine3d w_T_gaze;
    tf::poseMsgToEigen(rovi_gazebo::get_model_pose("camera_stereo"), w_T_gaze);

    // Get constant OPENGL transformation
    static Eigen::Matrix4f gaze_T_c  = ( Eigen::Matrix4f() << 0.f, 0.f, 1.f, 0.f,
                                                             -1.f, 0.f, 0.f, 0.f,
                                                              0.f,-1.f, 0.f, 0.f,
                                                              0.f, 0.f, 0.f, 1.f ).finished();

    // Define the overall transformation
    static Eigen::Matrix4f trans = (w_T_gaze.cast<float>()).matrix() * gaze_T_c;

    // Transform the point cloud
    pcl::transformPointCloud(*cloud_scene_ptr, *cloud_scene_ptr, trans);

    // Cropbox
    static pcl::CropBox<pcl::PointXYZRGBNormal> box_filter;
    {
        box_filter.setMin(Eigen::Vector4f(0.0, 0.85, 0.70, 1.0f));
        box_filter.setMax(Eigen::Vector4f(0.8, 1.25, 1.25, 1.0f));
        box_filter.setInputCloud(cloud_scene_ptr);
        box_filter.filter(*filtered_scene_ptr);
        *cloud_scene_ptr = *filtered_scene_ptr;
        //*cloud_orig_ptr = *cloud_scene_ptr;
    }

    // Plane fitting
    constexpr auto dist_tsh = 0.01f;
    static pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
    static pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
    static pcl::SACSegmentation<pcl::PointXYZRGBNormal> segm;
    {
        segm.setOptimizeCoefficients(true);
        segm.setModelType(pcl::SACMODEL_PLANE);
        segm.setMethodType(pcl::SAC_RANSAC);
        segm.setDistanceThreshold(dist_tsh);
        segm.setInputCloud(cloud_scene_ptr);
        segm.segment(*inliers, *coeff);

        ROS_INFO_STREAM("Number of inliers for plane_segmentation: " << inliers->indices.size());

        if (inliers->indices.size() != 0 )
        {
            double zmin = 0;
            for (const auto &idx : inliers->indices)
                zmin += cloud_scene_ptr->points[idx].z;
            zmin /= inliers->indices.size();

            // Create filtering object
            pcl::PassThrough<pcl::PointXYZRGBNormal> pass;
            pass.setInputCloud(cloud_scene_ptr);
            pass.setFilterFieldName("z");
            pass.setFilterLimits(zmin + 0.025, zmin + 1);
            pass.setFilterLimitsNegative(false);
            pass.filter(*filtered_scene_ptr);
            *cloud_scene_ptr = *filtered_scene_ptr;
        }
    }

    // Voxel Filter
    static pcl::VoxelGrid<pcl::PointXYZRGBNormal> voxel_filter;
    {
        voxel_filter.setInputCloud(cloud_scene_ptr);
        voxel_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
        voxel_filter.filter(*filtered_scene_ptr);
        *cloud_scene_ptr = *filtered_scene_ptr;
    }

    // Statistical outlier removal
    static pcl::StatisticalOutlierRemoval<pcl::PointXYZRGBNormal> sor;
    {
        sor.setInputCloud(cloud_scene_ptr);
        sor.setMeanK(10);
        sor.setStddevMulThresh(1);
        sor.filter(*filtered_scene_ptr);
        *cloud_scene_ptr = *filtered_scene_ptr;
    }

    // Estimate normals from PoV
    static pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> normal_est;
    {
        pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
        normal_est.setInputCloud(cloud_scene_ptr);
        normal_est.setViewPoint(trans(0,3), trans(1,3), trans(2,3));
        normal_est.setSearchMethod(tree);
        normal_est.setKSearch(25);
        normal_est.compute(*cloud_scene_ptr);
    }

    static pcl::SpinImageEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::Histogram<153>> spin;
    {
        spin.setInputCloud(cloud_scene_ptr);
        spin.setInputNormals(cloud_scene_ptr);
        spin.setRadiusSearch(0.15);
        spin.compute(*features_scene_ptr);
    }

    // Save the pointcloud
    // pcl::io::savePCDFileASCII("scene_voxel.pcd", *cloud_scene_ptr);
}

bool
M1::read_compute_features_object(const std::string & obj)
{

    int error = pcl::io::loadPCDFile<pcl::PointXYZRGBNormal>(obj, *cloud_object_ptr);

    if(error != 0)
    {
        return false;
    }

    // Compute the transformation matrix from world to gazebo camera
    static Eigen::Affine3d w_T_gaze;
    tf::poseMsgToEigen(rovi_gazebo::get_model_pose("camera_stereo"), w_T_gaze);

    // Get constant OPENGL transformation
    static Eigen::Matrix4f gaze_T_c  = ( Eigen::Matrix4f() << 0.f, 0.f, 1.f, 0.f,
                                                             -1.f, 0.f, 0.f, 0.f,
                                                              0.f,-1.f, 0.f, 0.f,
                                                              0.f, 0.f, 0.f, 1.f ).finished();
    // Define the overall transformation
    static Eigen::Matrix4f trans = (w_T_gaze.cast<float>()).matrix() * gaze_T_c;

    // Give some arbitrary colours to the object
    for (size_t i = 0; i < cloud_object_ptr->size(); i++)
    {
        std::uint8_t b = (std::uint8_t) 200;
        std::uint8_t g = (std::uint8_t) 150;
        std::uint8_t r = (std::uint8_t) 128;
        std::uint32_t rgb = ( (uint32_t) r << 24 | (uint32_t) g << 16 | (uint32_t) b << 8);
        cloud_object_ptr -> points[i].rgb = *reinterpret_cast<float*>(&rgb);
    }

    // Voxel Filter
    static pcl::VoxelGrid<pcl::PointXYZRGBNormal> voxel_filter;
    {
        voxel_filter.setInputCloud(cloud_object_ptr);
        voxel_filter.setLeafSize(leaf_size, leaf_size, leaf_size);
        voxel_filter.filter(*filtered_object_ptr);
        *cloud_object_ptr = *filtered_object_ptr;
    }

    // Estimate normals from PoV
    static pcl::NormalEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> normal_est;
    {
        pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGBNormal>());
        normal_est.setInputCloud(cloud_object_ptr);
        normal_est.setViewPoint(trans(0,3), trans(1,3), trans(2,3));
        normal_est.setSearchMethod(tree);
        normal_est.setKSearch(25);
        normal_est.compute(*cloud_object_ptr);
    }

    static pcl::SpinImageEstimation<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal, pcl::Histogram<153>> spin;
    {
        spin.setInputCloud(cloud_object_ptr);
        spin.setInputNormals(cloud_object_ptr);
        spin.setRadiusSearch(0.15);
        spin.compute(*features_object_ptr);
    }

    // Save the pointcloud
    //pcl::io::savePCDFileASCII("voxel_object.pcd", *cloud_object_ptr);

    return true;
}

void
M1::match_features()
{

    // Define the L2_norm operator
    static auto L2_norm = [](const pcl::Histogram<153> & f1, const pcl::Histogram<153> & f2)
    {
        float sum = 0.f;
        for(auto i = 0; i < f1.descriptorSize(); i++)
            sum += std::pow((f1.histogram[i]-f2.histogram[i]), 2);
        return sum;
    };

    // Define nearest neightbour
    static auto nearest_neighbour = [](const pcl::PointCloud<pcl::Histogram<153>>::Ptr & query, const int idx_query, const pcl::PointCloud<pcl::Histogram<153>>::Ptr & scene, int & idx_scene)
    {
        float min_dist = std::numeric_limits<float>::max();

        for(auto i = 0; i < scene->size(); i++)
        {
            float dist = L2_norm(query->points[idx_query], scene->points[i]);
            if (min_dist > dist)
            {
                min_dist = dist;
                idx_scene = i;
            }
        }

        return min_dist;
    };

    // Correspondences are reset.
    corr.clear();
    corr.resize(features_object_ptr->size());

    // Start matching
    int idx_scene = 0;
    for(auto idx_query = 0; idx_query < features_object_ptr->size(); idx_query++)
    {
        corr[idx_query].index_query    = idx_query;
        corr[idx_query].distance       = nearest_neighbour(features_object_ptr, idx_query, features_scene_ptr, idx_scene);
        corr[idx_query].index_match    = idx_scene;
    }
}

Eigen::Matrix4f
M1::ransac_features(const int & max_it)
{
    // Note
    ROS_INFO_STREAM("Amount of iterations: " << max_it);

    // Metrics
    int max_inliers = 0;
    Eigen::Matrix4f best_T, T;

    // Variables
    std::vector<int> query(3), scene(3);
    pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr aligned_object_ptr(new pcl::PointCloud<pcl::PointXYZRGBNormal>());

    // Random generator
    pcl::common::UniformGenerator<int> rnd_gen(0, corr.size() - 1);
    rnd_gen.setSeed(time(0));

    // Define search tree
    pcl::KdTreeFLANN<pcl::PointXYZRGBNormal> kd_tree_scene;
    kd_tree_scene.setInputCloud(cloud_scene_ptr);

    for(auto i = 0; i < max_it; i++)
    {
        // How far are we?
        if(i % 500 == 0)
        {
            ROS_INFO_STREAM("Iteration: " << i);
        }

        // Pick 3 points
        for (auto j = 0; j < 3; j++)
        {
            int random = rnd_gen.run();
            query[j] = corr[random].index_query;
            scene[j] = corr[random].index_match;
        }
        // Determine the transformation between cloud and cloud_object, we use the object and the scene
        pcl::registration::TransformationEstimationSVD<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal> svd;
        svd.estimateRigidTransformation(*cloud_object_ptr, query, *cloud_scene_ptr, scene, T);

        // Perform the transformation on the two clouds, here we usedaligned
        pcl::transformPointCloud(*cloud_object_ptr, *aligned_object_ptr, T);

        // Perform NN
        int K = 1;
        std::vector<int> point_idx_nn_search(K);
        std::vector<float> point_nn_L2(K);

        // Inlier
        int inliers = 0;

        for (auto j = 0; j < aligned_object_ptr->size(); j++)
        {
            if ( kd_tree_scene.nearestKSearch(aligned_object_ptr->points[j], K, point_idx_nn_search, point_nn_L2) > 0 )
            {
                if (point_nn_L2[0] < 0.00001f)
                {
                    inliers++;
                }
            }
        }

        if (inliers > max_inliers)
        {
            ROS_INFO_STREAM("Inliers: " << inliers);
            max_inliers = inliers;
            best_T = T;
        }
    }

    pcl::transformPointCloud(*cloud_object_ptr, *cloud_object_ptr, best_T);

    // pcl::visualization::PCLVisualizer viewer("vis");
    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_orig_ptr, "scene");
    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_object_ptr, "obj");
    // viewer.spin();

    pcl::IterativeClosestPoint<pcl::PointXYZRGBNormal, pcl::PointXYZRGBNormal > icp;
    icp.setInputSource(cloud_object_ptr);
    icp.setInputTarget(cloud_scene_ptr);
    icp.setRANSACOutlierRejectionThreshold(leaf_size*1.2);
    icp.setMaxCorrespondenceDistance(0.005);
    icp.setEuclideanFitnessEpsilon(1e-9);
    icp.setTransformationEpsilon(1e-9);
    icp.setMaximumIterations(20);
    icp.align(*cloud_object_ptr);

    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_orig_ptr, "scene");
    // viewer.addPointCloud<pcl::PointXYZRGBNormal>(cloud_object_ptr, "obj1");
    // viewer.spin();

    return icp.getFinalTransformation() * best_T; //;

}

geometry_msgs::Pose
M1::estimate_pose(const int & it, const bool & draw, const double & noise)
{
    static auto file_bottle_pcd = ros::package::getPath("rovi_gazebo") + std::string("/models/bottle/bottle.pcd");

    // Get stereo images
    auto cam_images = M1::get_image_data();

    cv::Mat gaussian_noise_left = cv::Mat::zeros(cam_images[0].size(), cam_images[0].type());
    cv::Mat gaussian_noise_right = gaussian_noise_left;

    std::vector<double> mean = { 0, 0, 0 };
    std::vector<double> std = { noise, noise, noise };

    cv::randn(gaussian_noise_left, mean, std);
    cv::randn(gaussian_noise_right, mean, std);

    if(draw)
    {
        cv::imwrite("left.jpg", gaussian_noise_left);
    }

    cam_images[0] += gaussian_noise_left;
    cam_images[1] += gaussian_noise_right;

    // Compute the disparity map
    cv::Mat point_cloud = M1::compute_disparitymap(cam_images[LEFT], cam_images[RIGHT]);

    if(draw)
    {
        cv::imwrite("bottle_left.jpg",  cam_images[0]);
        cv::imwrite("bottle_right.jpg", cam_images[1]);
        std::ofstream write_cloud("write_mat.mat", std::ios_base::out);
        write_cloud << cv::format(point_cloud, cv::Formatter::FMT_CSV) << std::endl;
        write_cloud.close();
    }

    M1::compute_pointcloud_scene(point_cloud, cam_images[0]);
    M1::read_compute_features_object(file_bottle_pcd);
    M1::match_features();

    auto T = Eigen::Affine3d(M1::ransac_features(it).cast<double>());
    geometry_msgs::Pose pose;
    tf::poseEigenToMsg(T, pose);

    return pose;
}

}


// Copyright 2017 Reinaldo Astudillo and Martin van Gijzen
//
// Permission is hereby granted, free of charge, to any person obtaining a
// copy of this software and associated documentation files (the "Software"),
// to deal in the Software without restriction, including without limitation
// the rights to use, copy, modify, merge, publish, distribute, sublicense,
// and/or sell copies of the Software, and to permit persons to whom the
// Software is furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
// OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
// DEALINGS IN THE SOFTWARE.
//
// IDR(s) for solving systems of linear equations, biorthogonal version.
//
// Reference:
// Martin B. van Gijzen and Peter Sonneveld, Algorithm 913: An Elegant IDR(s)
// Variant that Efficiently Exploits Bi-orthogonality Properties.
// ACM Transactions on Mathematical Software, Vol. 38, No. 1, pp. 5:1-5:19, 2011

#ifndef ITL_BIDR_S_INCLUDE
#define ITL_BIDR_S_INCLUDE

#include <boost/numeric/mtl/concept/collection.hpp>
#include <boost/numeric/mtl/vector/dense_vector.hpp>
#include <boost/numeric/mtl/operation/random.hpp>
#include <boost/numeric/mtl/operation/orth.hpp>
#include <boost/numeric/mtl/operation/resource.hpp>
#include <boost/numeric/mtl/matrix/strict_upper.hpp>
#include <boost/numeric/mtl/utility/exception.hpp>
#include <boost/numeric/mtl/utility/irange.hpp>
#include <boost/numeric/linear_algebra/identity.hpp>
#include <boost/numeric/mtl/interface/vpt.hpp>

namespace itl {
using namespace mtl;

/// Induced Dimension Reduction (IDR(s))
template < class LinearOperator, class HilbertSpaceX, class HilbertSpaceB,
	   class Preconditioner, class Iteration >
int idr_s(const LinearOperator& A, HilbertSpaceX& x, const HilbertSpaceB& b,
	     const Preconditioner& M, Iteration& iter, size_t s)
{
    typedef typename mtl::Collection<HilbertSpaceX>::value_type Scalar;
    typedef HilbertSpaceX                                       Vector;
    mtl::vampir_trace<7010> tracer;

    if (size(b) == 0) throw mtl::logic_error("empty rhs vector");
    if (s < 1) s= 1;

    const Scalar                zero= math::zero(Scalar());
    const Scalar                one= math::one(Scalar());
    const double angle = 0.7;
    Vector                      v(resource(x)), t(resource(x)), r(b - A * x);
    mat::multi_vector<Vector>   G(Vector(resource(x), zero), s), U(Vector(resource(x), zero), s), P(Vector(resource(x), zero), s);
    dense_vector<Scalar>   f(s), c(s);
    mat::dense2D<Scalar>        Ms(s, s); Ms = one;
    double om = 1.0;
    random(P);
    orth(P);
    // Main iteration loop, build G-spaces:
    while (! iter.finished(r)) {
         // New righ-hand size for small system:
         f = trans(P) * r;
         for (size_t k= 0; k < s; k++) {
            // Solve small system and make v orthogonal to P
            irange range(k, s);
            c[range] = lower_trisolve(Ms[range][range], f[range]);
            // Preconditioning
            v = solve(M, r - G.vector(range)*c[range]);
            // Compute new U(:,k) and G(:,k), G(:,k) is in space G_j
            U.vector(k) = U.vector(range)*c[range] + om*v;
            // Matvec
            G.vector(k) = A*U.vector(k);
            // Bi-Orthogonalise the new basis vectors
            for (size_t i= 0; i < k; i++) {
                Scalar alpha = dot(P.vector(i), G.vector(k))/Ms[i][i];
                G.vector(k) = G.vector(k) - alpha*G.vector(i);
                U.vector(k) = U.vector(k) - alpha*U.vector(i);
            }
            // New column of M = P'*G  (first k-1 entries are zero)
            for (size_t i= k; i < s; i++) {
                Ms[i][k] = dot(P.vector(i), G.vector(k));
            }
            // Make r orthogonal to g_i, i = 1..k
            Scalar beta = f[k] / Ms[k][k];
            r = r - beta*G.vector(k);
            x = x + beta*U.vector(k);
            if ((++iter).finished(r))
                return iter;
            if (k < s-1) {
               for (size_t i= k+1; i < s; i++) {
                   f[i] = f[i] - beta*Ms[i][k];
               }
            }
         }
         // Now we have sufficient vectors in G_j to compute residual in G_j+1
         // Note: r is already perpendicular to P so v = r
         // Preconditioning
         v = solve(M, r);
         // Matvec
         t = A*v;
         // Computation of a new omega
         double ns = two_norm(r);
         double nt = two_norm(t);
         double ts = dot(t, r);
         double rho = fabs(ts/(nt*ns));
         om=ts/(nt*nt);
         if (rho < angle)
             om = om*angle/rho;
         r = r - om*t;
         x = x + om*v;
         ++iter;
    }
    return iter;
}

} // namespace itl

#endif // ITL_BIDR_S_INCLUDE


/*
 * ServerPokerAlgorithmDelegate.cpp
 *
 *  Created on: 2013-7-31
 *      Author: caiqingfeng
 */
#include <boost/random.hpp>
#include <boost/generator_iterator.hpp>
#include <boost/regex.hpp>

#include "ServerPokerAlgorithmDelegate.h"
#include "holdem/src/LYHandStrength.h"

typedef boost::mt19937 LYRng; //random number generator

ServerPokerAlgorithmDelegate *ServerPokerAlgorithmDelegate::_instance = NULL;

ServerPokerAlgorithmDelegate::ServerPokerAlgorithmDelegate() {
	// TODO Auto-generated constructor stub

}

ServerPokerAlgorithmDelegate::~ServerPokerAlgorithmDelegate() {
	// TODO Auto-generated destructor stub
}

ServerPokerAlgorithmDelegate *ServerPokerAlgorithmDelegate::Instance() {
	// TODO Auto-generated constructor stub
	if (NULL == _instance) {
		_instance = new ServerPokerAlgorithmDelegate();
	}
	return _instance;
}

void ServerPokerAlgorithmDelegate::doShuffle(std::vector<LYCard> &deck)
{
	LYRng rng(std::time(0)); //must set a seed, otherwise always get the same result
	boost::uniform_int<> deckRange(1, 52);
	boost::variate_generator<LYRng, boost::uniform_int<> > oneDeck(rng, deckRange);
	std::random_shuffle(deck.begin(), deck.end(), oneDeck);
}

//LYHandStrengthPtr ServerPokerAlgorithmDelegate::genHandStrength(LYHoldemSeat *seat,
//			const std::vector<LYCard> &communityCards)
//{
//}


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

#include <Eigen/Dense>
#include <cassert>
#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <cuda.h>
#include <cuda_runtime_api.h>
#include <iostream>
#include <string>
#include <unsupported/Eigen/CXX11/Tensor>
#include <vector>

#include "device_ptr.h"
#include "haste.h"

using haste::v0::gru::ForwardPass;
using haste::v0::gru::BackwardPass;
using std::string;

using Tensor1 = Eigen::Tensor<float, 1>;
using Tensor2 = Eigen::Tensor<float, 2>;
using Tensor3 = Eigen::Tensor<float, 3>;

constexpr int BATCH_SIZE = 64;
constexpr int SEQUENCE_LEN = 1000;
constexpr int HIDDEN_DIMS = 512;
constexpr int INPUT_DIMS = 512;

static cublasHandle_t g_blas_handle;

class ScopeTimer {
  public:
    ScopeTimer(const string& msg) : msg_(msg) {
      cudaEventCreate(&start_);
      cudaEventCreate(&stop_);
      cudaDeviceSynchronize();
      cudaEventRecord(start_);
    }

    ~ScopeTimer() {
      float elapsed_ms;
      cudaEventRecord(stop_);
      cudaEventSynchronize(stop_);
      cudaEventElapsedTime(&elapsed_ms, start_, stop_);
      printf("%s %fms\n", msg_.c_str(), elapsed_ms);
      cudaEventDestroy(start_);
      cudaEventDestroy(stop_);
    }

  private:
    string msg_;
    cudaEvent_t start_, stop_;
};

void GruInference(
    const Tensor2& W,
    const Tensor2& R,
    const Tensor1& bx,
    const Tensor1& br,
    const Tensor3& x) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  // Copy weights over to GPU.
  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor1> bx_dev(bx);
  device_ptr<Tensor1> br_dev(br);
  device_ptr<Tensor3> x_dev(x);

  device_ptr<Tensor2> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> tmp_Wx_dev(time_steps * batch_size * hidden_size * 3);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 3);

  h_dev.zero();

  ScopeTimer t("Inference:");

  ForwardPass<float> forward = ForwardPass<float>(
      false,  // training
      batch_size,
      input_size,
      hidden_size,
      g_blas_handle);

  forward.Run(
      time_steps,
      W_dev.data,
      R_dev.data,
      bx_dev.data,
      br_dev.data,
      x_dev.data,
      h_dev.data,
      nullptr,
      tmp_Wx_dev.data,
      tmp_Rh_dev.data,
      0.0f,
      nullptr);
}

void GruTrain(
    const Tensor2& W,
    const Tensor2& R,
    const Tensor1& bx,
    const Tensor1& br,
    const Tensor3& x,
    const Tensor3& dh_new) {
  const int time_steps = x.dimension(2);
  const int batch_size = x.dimension(1);
  const int input_size = x.dimension(0);
  const int hidden_size = R.dimension(1);

  // Copy weights over to GPU.
  device_ptr<Tensor2> W_dev(W);
  device_ptr<Tensor2> R_dev(R);
  device_ptr<Tensor1> bx_dev(bx);
  device_ptr<Tensor1> br_dev(br);
  device_ptr<Tensor3> x_dev(x);
  device_ptr<Tensor3> dh_new_dev(dh_new);

  device_ptr<Tensor2> h_dev((time_steps + 1) * batch_size * hidden_size);
  device_ptr<Tensor3> tmp_Wx_dev(time_steps * batch_size * hidden_size * 3);
  device_ptr<Tensor2> tmp_Rh_dev(batch_size * hidden_size * 3);
  device_ptr<Tensor3> v_dev(time_steps * batch_size * hidden_size * 4);

  h_dev.zero();

  {
    ScopeTimer t("Train forward:");
    ForwardPass<float> forward = ForwardPass<float>(
        true,  // training
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    forward.Run(
        time_steps,
        W_dev.data,
        R_dev.data,
        bx_dev.data,
        br_dev.data,
        x_dev.data,
        h_dev.data,
        v_dev.data,
        tmp_Wx_dev.data,
        tmp_Rh_dev.data,
        0.0f,
        nullptr);
  }

  device_ptr<Tensor3> dx_dev(time_steps * batch_size * input_size);
  device_ptr<Tensor2> dW_dev(input_size * hidden_size * 3);
  device_ptr<Tensor2> dR_dev(hidden_size * hidden_size * 3);
  device_ptr<Tensor1> dbx_dev(hidden_size * 3);
  device_ptr<Tensor1> dbr_dev(hidden_size * 3);
  device_ptr<Tensor2> dh_dev(batch_size * hidden_size);
  device_ptr<Tensor3> dp_dev(time_steps * batch_size * hidden_size * 3);
  device_ptr<Tensor3> dq_dev(time_steps * batch_size * hidden_size * 3);

  {
    ScopeTimer t("Train backward:");
    BackwardPass<float> backward(
        batch_size,
        input_size,
        hidden_size,
        g_blas_handle);

    backward.Run(
        time_steps,
        W_dev.data,
        R_dev.data,
        bx_dev.data,
        br_dev.data,
        x_dev.data,
        h_dev.data,
        v_dev.data,
        dh_new_dev.data,
        dx_dev.data,
        dW_dev.data,
        dR_dev.data,
        dbx_dev.data,
        dbr_dev.data,
        dh_dev.data,
        dp_dev.data,
        dq_dev.data,
        nullptr);
  }
}

int main() {
  srand(time(0));

  cublasCreate(&g_blas_handle);

  // Weights.
  Tensor2 W(HIDDEN_DIMS * 3, INPUT_DIMS);
  Tensor2 R(HIDDEN_DIMS * 3, HIDDEN_DIMS);
  Tensor1 bx(HIDDEN_DIMS * 3);
  Tensor1 br(HIDDEN_DIMS * 3);

  // Input.
  Tensor3 x(INPUT_DIMS, BATCH_SIZE, SEQUENCE_LEN);

  // Gradients from upstream layers.
  Tensor3 dh(HIDDEN_DIMS, BATCH_SIZE, SEQUENCE_LEN + 1);

  W.setRandom();
  R.setRandom();
  bx.setRandom();
  br.setRandom();
  x.setRandom();
  dh.setRandom();

  GruInference(W, R, bx, br, x);
  GruTrain(W, R, bx, br, x, dh);

  cublasDestroy(g_blas_handle);

  return 0;
}


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TWO_PROD_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_TWO_PROD_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/two_prod.hpp>
#include <boost/simd/sdk/simd/logical.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/functions/simd/minus.hpp>
#include <boost/simd/include/functions/simd/is_inf.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/two_split.hpp>
#include <boost/simd/include/functions/simd/multiplies.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/if_zero_else.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/toolbox/arithmetic/functions/two_prod.hpp>
#include <boost/fusion/tuple.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0)(X),
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                         )
  {
    typedef int result_type;
    inline result_type operator()(A0 const& a,A0 const& b,
                                  A0 & r0,A0 & r1) const
    {
      typedef typename meta::as_logical<A0>::type bA0;
      r0 = a*b;
      A0 a1, a2, b1, b2;
      bA0 isinf = logical_and(logical_or(is_inf(b), is_inf(a)), is_inf(r0));
      two_split(a, a1, a2);
      two_split(b, b1, b2);
      r1 = if_zero_else(isinf, a2*b2 -(((r0-a1*b1)-a2*b1)-a1*b2));
      return 0;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                          (A0)(X),
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    inline result_type operator()(A0 const& a0,A0 const& a1,A0 & a3) const
    {
      A0 a2;
      two_prod(a0, a1, a2, a3);
      return a2;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::two_prod_, tag::cpu_,
                           (A0)(X),
                           ((simd_<floating_<A0>,X>))
                           ((simd_<floating_<A0>,X>))
                          )
  {
    typedef typename boost::fusion::tuple<A0,A0> result_type;

    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      result_type res;
      two_prod(a0,a1, boost::fusion::at_c<0>(res),boost::fusion::at_c<1>(res));
      return res;
    }
  };
} } }
#endif