// Boost.Geometry

// Copyright (c) 2020, Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_RELATE_CARTESIAN_HPP
#define BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_RELATE_CARTESIAN_HPP

#include <boost/geometry/strategies/relate/cartesian.hpp>

#include <boost/geometry/extensions/nsphere/strategies/cartesian/disjoint.hpp>
#include <boost/geometry/extensions/nsphere/strategies/cartesian/nsphere_in_box.hpp>
#include <boost/geometry/extensions/nsphere/strategies/cartesian/point_in_nsphere.hpp>

namespace boost { namespace geometry
{

namespace strategies { namespace relate
{

template <typename CalculationType = void>
class cartesian_for_nsphere
    : public strategies::relate::cartesian<CalculationType>
{
public:

    // covered_by

    template <typename Geometry1, typename Geometry2>
    static auto covered_by(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    util::is_pointlike<Geometry1>::value
                                 && std::is_same<nsphere_tag, typename tag<Geometry2>::type>::value
                                > * = nullptr)
    {
        return strategy::within::point_in_nsphere
            <
                Geometry1, Geometry2,
                strategy::within::point_nsphere_covered_by_comparable_distance
            >();
    }

    template <typename Geometry1, typename Geometry2>
    static auto covered_by(Geometry1 const&, Geometry2 const&,
                           std::enable_if_t
                                <
                                    std::is_same<nsphere_tag, typename tag<Geometry1>::type>::value
                                 && util::is_box<Geometry2>::value
                                > * = nullptr)
    {
        return strategy::within::nsphere_in_box
            <
                Geometry1, Geometry2,
                strategy::within::nsphere_covered_by_range
            >();
    }

    // within

    template <typename Geometry1, typename Geometry2>
    static auto within(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                util::is_pointlike<Geometry1>::value
                                && std::is_same<nsphere_tag, typename tag<Geometry2>::type>::value
                            > * = nullptr)
    {
        return strategy::within::point_in_nsphere
            <
                Geometry1, Geometry2,
                strategy::within::point_nsphere_within_comparable_distance
            >();
    }

    template <typename Geometry1, typename Geometry2>
    static auto within(Geometry1 const&, Geometry2 const&,
                       std::enable_if_t
                            <
                                std::is_same<nsphere_tag, typename tag<Geometry1>::type>::value
                                && util::is_box<Geometry2>::value
                            > * = nullptr)
    {
        return strategy::within::nsphere_in_box
            <
                Geometry1, Geometry2,
                strategy::within::nsphere_within_range
            >();
    }
};

namespace services
{

template <typename P, typename T, typename Geometry2>
struct default_strategy<model::nsphere<P, T>, Geometry2, cartesian_tag, cartesian_tag>
{
    using type = strategies::relate::cartesian_for_nsphere<>;
};

template <typename Geometry1, typename P, typename T>
struct default_strategy<Geometry1, model::nsphere<P, T>, cartesian_tag, cartesian_tag>
{
    using type = strategies::relate::cartesian_for_nsphere<>;
};

template <typename P1, typename T1, typename P2, typename T2>
struct default_strategy<model::nsphere<P1, T1>, model::nsphere<P2, T2>, cartesian_tag, cartesian_tag>
{
    using type = strategies::relate::cartesian_for_nsphere<>;
};

} // namespace services

}} // namespace strategies::relate

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_EXTENSIONS_NSPHERE_STRATEGIES_RELATE_CARTESIAN_HPP


// Created by BlurringShadow at 2021-03-13-下午 12:10

#include <boost/test/unit_test.hpp>

#include "systems/note/locator_system.h"

using namespace polycumic::game_core;

BOOST_AUTO_TEST_CASE(locator_system_test)
{
    systems::note::locator_system system{};
    const auto id = registry.create();
    registry.emplace<components::note::locator>(
        id,
        2250ms,
        1.0,
        components::note::surface::front,
        components::note::coordinate::left,
        components::note::coordinate::left
    );

    const auto& transform = registry.emplace<components::transform>(id);
    registry.emplace<components::note::direction>(id);
    registry.emplace<components::note::judge_state>(id);

    system.set_timestamp(0ms);
    const auto excepted_res = scale(
        translate(
            glm::one<glm::dmat4>(),
            glm::dvec3{150, 100, 100} * 0.25
        ),
        mix({}, glm::one<glm::dvec3>(), 0.25)
    );

    const auto epsilon = 0.001;

    for(auto i = 0; i < excepted_res.length(); ++i)
        for(auto j = 0; j < glm::dvec3::length(); ++j)
        {
            BOOST_CHECK_CLOSE(excepted_res[i][j], transform.value[i][j], 0.001);
        }
}


// Copyright (C) 2019 David Harmon and Artificial Necessity
// This code distributed under zlib, see LICENSE.txt for terms.

#pragma once

#include "bvh.hpp"
#include <Eigen/Core>
#include <igl/edge_flaps.h>
#include <igl/readOBJ.h>
#include <queue>

#include "timer.hpp"

struct TriMesh {
    using VecXr = Eigen::Matrix<double, Eigen::Dynamic, 1>;
    using Real = double;

    Eigen::MatrixXi f;
    Eigen::MatrixXi ft;
    Eigen::VectorXd fn;
    Eigen::VectorXd x;
    Eigen::VectorXd u;
    Eigen::VectorXd vt;
    Eigen::VectorXd vn;

    Eigen::MatrixXi e;  // edges (#E x 2)
    Eigen::MatrixXi ef; // edge-face (#E x 2)
    Eigen::MatrixXi ei; // Edge opp vertices (#e x 2)

    Eigen::MatrixXi s; // stitches (#S x 2)

    Eigen::VectorXd v;
    Eigen::VectorXd m;
    Eigen::VectorXi vl;
    Eigen::VectorXi fl;

    std::vector<std::array<float, 3>> c; // Colors

    Eigen::VectorXi flags;

    BVH<2> bvh;

    int idx;

    std::unordered_set<int> fixed;

    bool has_uvs() const {
        return f.rows() == ft.rows() && (x.size() / 3) == (vt.size() / 2);
    }

    // Takes a vertex index and returns an index into vt for that vertex
    int uv_index(int idx) const {
        for (int i=0; i<f.rows(); i++) {
            for (int j=0; j<3; j++) {
                if (f(i,j) == idx) {
                    return ft(i,j);
                }
            }
        }
        return -1;
    }

    int edge_index(int v0, int v1) const {
        for (int i=0; i<e.rows(); i++) {
            if ((e(i,0) == v0 && e(i,1) == v1) ||
                (e(i,0) == v1 && e(i,1) == v0)) {
                return i;
            }
        }
        return -1;
    }

    int n_vertices() const { return x.size() / 3; }

    int n_edges() const { return e.rows(); }

    int n_triangles() const { return f.rows(); }

    int n_stitches() const { return s.rows(); }

    void update_normals() {
        vn.setZero();

        for (int i=0; i<f.rows(); i++) {
            fn.segment<3>(3*i) = (x.segment<3>(3*f(i,1)) -
                                  x.segment<3>(3*f(i,0))).cross(
                                  x.segment<3>(3*f(i,2)) -
                                  x.segment<3>(3*f(i,0))).normalized();

            for (int j=0; j<3; j++) {
                vn.segment<3>(3*f(i,j)) += fn.segment<3>(3*i);
            }
        }

        for (int i=0; i<n_vertices(); i++) {
            vn.segment<3>(3*i).normalize();
        }
    }

    void reset_colors(const Eigen::Vector3f& def_color) {
        c.resize(x.size() / 3);

        const std::array<float, 3> col = { def_color[0], def_color[1], def_color[2] };

        for (int i=0; i<x.size()/3; i++) {
            c[i] = col;
        }
    }

    void color_flagged(const Eigen::Vector3f& flag_color) {
        const std::array<float, 3> col = { flag_color[0], flag_color[1], flag_color[2] };
        for (int i=0; i<flags.size(); i++) {
            if (flags[i]) {
                c[i] = col;
            }
        }
    }
};

template <typename Real>
struct MeshBase
{
    using Vec2r = Eigen::Matrix<Real, 2, 1>;
    using Vec3r = Eigen::Matrix<Real, 3, 1>;
    using VecXr = Eigen::Matrix<Real, Eigen::Dynamic, 1>;
    using Mat3r = Eigen::Matrix<Real, 3, 3>;
    using MatXr = Eigen::Matrix<Real, Eigen::Dynamic, Eigen::Dynamic>;
};

class SimMesh : public MeshBase<double>
{
public:
    SimMesh(TriMesh& mesh) : mesh_(mesh) { }

    const TriMesh& tri_mesh() const { return mesh_; }
    TriMesh& tri_mesh() { return mesh_; }

public:
    using Real = double;

    Vec3r ks_stretch = Vec3r(1.0e5, 1.0e5, 1.0e5);
    Vec3r kd_stretch = Vec3r::Zero();
    Vec2r ks_bend = Vec2r(0.01, 0.01);
    Vec2r kd_bend = Vec2r::Zero();

    Real ks_stitch = 1.0e5;
    Real kd_stitch = 1.5e2;

    Real mu = 0.0;

    // Required functions and types
public:
    // Store row index as the iterator
    struct Vertex
    {
        using iterator = int;

        static constexpr int size = 1;
    };

    struct Edge
    {
        using iterator = int;

        static constexpr int size = 2;
    };

    struct Triangle
    {
        using iterator = int;

        static constexpr int size = 3;
    };

    struct Stitch
    {
        using iterator = int;

        static constexpr int size = 2;
    };

    struct Diamond
    {
        using iterator = int;

        static constexpr int size = 4;
    };

    template <typename Element>
    int n_elements() const { assert(false && "Not implemented!"); return 0; }

    template<>
    int n_elements<Vertex>() const { return mesh_.n_vertices(); }

    template<>
    int n_elements<Edge>() const { return mesh_.n_edges(); }

    template<>
    int n_elements<Triangle>() const { return mesh_.n_triangles(); }

    template<>
    int n_elements<Stitch>() const { return mesh_.n_stitches(); }

    template<>
    int n_elements<Diamond>() const { return mesh_.n_edges(); }

    template <typename Element>
    typename Element::iterator begin() const { return 0; }

    template <typename Element>
    typename Element::iterator end() const { return n_elements<Element>(); }

    int vidx(Vertex::iterator vh) const { return vh; }

    template <typename Element>
    std::array<Vertex::iterator, Element::size> vertices(typename Element::iterator it) const
    { assert(false && "Vertex accessor not implemented!"); }

    template <>
    std::array<Vertex::iterator, Vertex::size> vertices<Vertex>(typename Vertex::iterator it) const
    { return { it }; }

    template <>
    std::array<Vertex::iterator, Edge::size> vertices<Edge>(typename Edge::iterator it) const
    { return { mesh_.e(it, 0), mesh_.e(it, 1) }; }

    template <>
    std::array<Vertex::iterator, Triangle::size> vertices<Triangle>(typename Triangle::iterator it) const
    { return { mesh_.f(it, 0), mesh_.f(it, 1), mesh_.f(it, 2) }; }

    template <>
    std::array<Vertex::iterator, Stitch::size> vertices<Stitch>(Stitch::iterator it) const
    { return { mesh_.s(it, 0), mesh_.s(it, 1) }; }

    template <>
    std::array<Vertex::iterator, Diamond::size> vertices<Diamond>(Diamond::iterator it) const  {
        return { mesh_.e (it, 0), mesh_.e (it, 1),
                 mesh_.ei(it, 0), mesh_.ei(it, 1) };
    }

    // Vertex
    Vec3r x(Vertex::iterator vh) const // Current vertex position
    { return mesh_.x.segment<3>(3*vh); }

    template <typename Derived>
    void set_x(Vertex::iterator vh, const Eigen::MatrixBase<Derived>& x_new)
    { mesh_.x.segment<3>(3*vh) = x_new; }

    Vec3r u(Vertex::iterator vh) const // Undeformed vertex position
    { return Vec3r(mesh_.u[2*vh], mesh_.u[2*vh+1], 0.0); }

    Vec3r v(Vertex::iterator vh) const // Current vertex velocity
    { return mesh_.v.segment<3>(3*vh); }

    template <typename Derived>
    void set_v(Vertex::iterator vh, const Eigen::MatrixBase<Derived>& v_new)
    { mesh_.v.segment<3>(3*vh) = v_new; }

    Real mass(Vertex::iterator vh) const  // Vertex mass
    { return mesh_.m[3*vh]; }

    Real friction(Vertex::iterator vh) const { return mu; }

    bool is_fixed(Vertex::iterator vh) const { return mesh_.fixed.count(vh); }

    Real ks_x(Vertex::iterator vh) const { return ks_stretch.x(); }
    Real ks_y(Vertex::iterator vh) const { return ks_stretch.y(); }
    Real ks_shear(Vertex::iterator vh) const { return ks_stretch.z(); }

    Real kd_x(Vertex::iterator vh) const { return kd_stretch.x(); }
    Real kd_y(Vertex::iterator vh) const { return kd_stretch.y(); }
    Real kd_shear(Vertex::iterator vh) const { return kd_stretch.z(); }

    // Edge
    std::array<Vertex::iterator, 2> edge_vertices(Edge::iterator eh) const
    { return { mesh_.e(eh, 0), mesh_.e(eh, 1) }; }

    Real shrinkage(Edge::iterator eh) const
    { return 1.0; }

    Real stretch_ks(Edge::iterator eh) const
    { return ks_stretch.x(); }

    Real stretch_kd(Edge::iterator eh) const
    { return kd_stretch.x(); }

    Real kb_x(Edge::iterator eh) const { return ks_bend.x(); }
    Real kb_y(Edge::iterator eh) const { return ks_bend.y(); }
    Real kbd_x(Edge::iterator eh) const { return kd_bend.x(); }
    Real kbd_y(Edge::iterator eh) const { return kd_bend.y(); }

    // Stitch
    std::array<Vertex::iterator, 2> stitch_vertices(Stitch::iterator sh) const
    { return { mesh_.s(sh, 0), mesh_.s(sh, 1) }; }

    Real stitch_ks(Stitch::iterator sh) const { return ks_stitch; }
    Real stitch_kd(Stitch::iterator sh) const { return kd_stitch; }

    std::array<Vertex::iterator, 4> diamond_vertices(Diamond::iterator dh) const {
        return { mesh_.e (dh, 0), mesh_.e (dh, 1),
                 mesh_.ei(dh, 0), mesh_.ei(dh, 1) };
    }

    Real bend_ks(Diamond::iterator dh) const
    { return ks_bend.x(); }

    Real bend_kd(Diamond::iterator dh) const
    { return kd_bend.x(); }

    //Real stretch_ks(Triangle::iterator eh) const
    //{ return ks_stretch.x(); }

    //Real stretch_kd(Triangle::iterator eh) const
    //{ return kd_stretch.x(); }

protected:
    TriMesh& mesh_;
};

inline
void load_tri_mesh(const std::string& fn, TriMesh& mesh, bool get_edges=false) {
    Eigen::MatrixXd V;
    Eigen::MatrixXd VT;
    Eigen::MatrixXd VN;
    Eigen::MatrixXi FN;

    igl::readOBJ(fn, V, VT, VN, mesh.f, mesh.ft, FN);

    mesh.x = Eigen::VectorXd(3 * V.rows());
    mesh.vt = Eigen::VectorXd(2 * VT.rows());

    for (int i=0; i<V.rows(); i++) {
        mesh.x.segment<3>(3*i) = V.row(i);
    }

    for (int i=0; i<VT.rows(); i++) {
        mesh.vt.segment<2>(2*i) = VT.row(i);
    }

    if (get_edges) {
        Eigen::VectorXi emap;
        igl::edge_flaps(mesh.f, mesh.e, emap, mesh.ef, mesh.ei);
    }

    mesh.fn = Eigen::VectorXd(mesh.f.rows() * 3);
    mesh.vn = Eigen::VectorXd(mesh.x.size());

    mesh.flags = Eigen::VectorXi::Zero(mesh.x.size() / 3);

    mesh.update_normals();
    mesh.reset_colors(Eigen::Vector3f(0.0f, 0.0f, 1.0f));
}

struct AnimatedMesh : TriMesh {

    void next_frame(double dt) {
        if (vertex_data.size()) {
            Eigen::VectorXd old_x = x;

            x = Eigen::Map<Eigen::VectorXd>(vertex_data.front().data(), x.size());
            vertex_data.pop();

            bvh.refit(f, x, 2.5);

            v = (x - old_x) / dt;
        } else {
            v.setZero();
        }
    }

    std::queue<std::vector<double>> vertex_data;
};


#ifndef STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_RNG_HPP
#define STAN_MATH_PRIM_MAT_PROB_CATEGORICAL_RNG_HPP

#include <boost/random/uniform_01.hpp>
#include <boost/random/variate_generator.hpp>
#include <stan/math/prim/mat/err/check_simplex.hpp>
#include <stan/math/prim/scal/err/check_bounded.hpp>
#include <stan/math/prim/scal/fun/value_of.hpp>
#include <stan/math/prim/mat/fun/sum.hpp>
#include <stan/math/prim/mat/fun/cumulative_sum.hpp>
#include <stan/math/prim/mat/meta/index_type.hpp>
#include <stan/math/prim/scal/fun/constants.hpp>
#include <stan/math/prim/scal/meta/include_summand.hpp>

namespace stan {
namespace math {

template <class RNG>
inline int categorical_rng(
    const Eigen::Matrix<double, Eigen::Dynamic, 1>& theta, RNG& rng) {
  using boost::uniform_01;
  using boost::variate_generator;

  static const char* function = "categorical_rng";

  check_simplex(function, "Probabilities parameter", theta);

  variate_generator<RNG&, uniform_01<> > uniform01_rng(rng, uniform_01<>());

  Eigen::VectorXd index(theta.rows());
  index.setZero();

  index = cumulative_sum(theta);

  double c = uniform01_rng();
  int b = 0;
  while (c > index(b, 0))
    b++;
  return b + 1;
}
}  // namespace math
}  // namespace stan
#endif


#ifndef CONTEXT_HELIB_F2_HPP
#define CONTEXT_HELIB_F2_HPP

#include <NTL/BasicThreadPool.h>
#include <chrono>
#include <type_traits>
#include <unordered_map>
#include <complex>
#include <cmath>
#include "bits.hpp"
NTL_CLIENT

#include "circuit.hpp"
#include "context.hpp"

#include "Ctxt.h"
#include "EncryptedArray.h"
#include "FHE.h"

#include "binaryArith.h"
#include "binaryCompare.h"
#include "intraSlot.h"

namespace SHEEP {

template <typename PlaintextT, typename CiphertextT>
class ContextHElib : public Context<PlaintextT, CiphertextT> {
 public:
  typedef PlaintextT Plaintext;
  typedef CiphertextT Ciphertext;
  // typedef std::vector<PlaintextT> Ciphertext;
  // typedef PlaintextT CiphertextEl;

  /// constructors
  ContextHElib(long p,               // plaintext modulus
               long param_set = 0,   // parameter set, from 0 (tiny) to 4 (huge)
               long bootstrapl = 1,  // bootstrap or not?
               long hamming_weight = 128)
      :  // Hamming weight of secret key

        m_p(p),
        m_param_set(param_set),
        m_bootstrapl(bootstrapl),
        m_w(hamming_weight) {
    /// BITWIDTH(bool) is 8, so need to deal with this by hand...
    //// (better to specialize class?)
    if (std::is_same<Plaintext, bool>::value)
      m_bitwidth = 1;
    else
      m_bitwidth = BITWIDTH(Plaintext);

    ////  populate the map that will allow us to set parameters via an input
    ///file (or string)

    this->m_param_name_map.insert({"BaseParamSet", m_param_set});
    this->m_param_name_map.insert({"HammingWeight", m_w});
    this->m_param_name_map.insert({"Bootstrap", m_bootstrapl});
    this->m_param_name_map.insert({"m", m_m});
    this->m_param_name_map.insert({"phim", m_phim});
    this->m_param_name_map.insert({"d", m_d});
    this->m_param_name_map.insert({"m1", m_m1});
    this->m_param_name_map.insert({"m2", m_m2});
    this->m_param_name_map.insert({"m3", m_m3});
    this->m_param_name_map.insert({"g1", m_g1});
    this->m_param_name_map.insert({"g2", m_g2});
    this->m_param_name_map.insert({"g3", m_g3});
    this->m_param_name_map.insert({"ord1", m_ord1});
    this->m_param_name_map.insert({"ord2", m_ord2});
    this->m_param_name_map.insert({"ord3", m_ord3});
    this->m_param_name_map.insert({"c", m_c});
    this->m_param_name_map.insert({"BitsPerLevel", m_B});
    this->m_param_name_map.insert({"Levels", m_L});
    /// sizes of objects in bytes.  Assign values when they are constructed.
    this->m_ciphertext_size = 0;
    this->m_public_key_size = 0;
    this->m_private_key_size = 0;

    /// configure
    configure();
  }

  void configure() {
    m_bootstrap = (bool)m_bootstrapl;
    /// Set all the other parameters.

    long mValues[][15] = {
        // { p, phi(m),   m,   d, m1, m2, m3,    g1,   g2,   g3, ord1,ord2,ord3,
        // B,c}
        {2, 48, 105, 12, 3, 35, 0, 71, 76, 0, 2, 2, 0, 25, 2},
        {2, 600, 1023, 10, 11, 93, 0, 838, 584, 0, 10, 6, 0, 25, 2},
        {2, 2304, 4641, 24, 7, 3, 221, 3979, 3095, 3760, 6, 2, -8, 25, 3},
        {2, 15004, 15709, 22, 23, 683, 0, 4099, 13663, 0, 22, 31, 0, 25, 3},
        {2, 27000, 32767, 15, 31, 7, 151, 11628, 28087, 25824, 30, 6, -10, 28,
         4}};

    long* vals = mValues[m_param_set];
    //// if parameters were not set explicitly in a parameters file, take the
    ///value from /  the mValues array.  (But if they were set explicitly, use
    ///that value!)

    if (!this->override_param("m")) m_m = vals[2];
    if (!this->override_param("phi(m)")) m_phim = vals[1];
    if (!this->override_param("d")) m_d = vals[3];
    if (!this->override_param("m1")) m_m1 = vals[4];
    if (!this->override_param("m2")) m_m2 = vals[5];
    if (!this->override_param("m3")) m_m3 = vals[6];
    if (!this->override_param("g1")) m_g1 = vals[7];
    if (!this->override_param("g2")) m_g2 = vals[8];
    if (!this->override_param("g3")) m_g3 = vals[9];
    if (!this->override_param("ord1")) m_ord1 = vals[10];
    if (!this->override_param("ord2")) m_ord2 = vals[11];
    if (!this->override_param("ord3")) m_ord3 = vals[12];
    if (!this->override_param("B")) m_B = vals[13];
    if (!this->override_param("c")) m_c = vals[14];

    NTL::Vec<long> mvec;
    append(mvec, m_m1);
    if (m_m2 > 1) append(mvec, m_m2);
    if (m_m3 > 1) append(mvec, m_m3);

    std::vector<long> gens;
    gens.push_back(m_g1);
    if (m_g2 > 1) gens.push_back(m_g2);
    if (m_g3 > 1) gens.push_back(m_g3);

    std::vector<long> ords;
    ords.push_back(m_ord1);
    if (abs(m_ord2) > 1) ords.push_back(m_ord2);
    if (abs(m_ord3) > 1) ords.push_back(m_ord3);

    /// number of levels  (copied from HElib's Test_binaryCompare)
    if (!this->override_param("Levels")) {
      if (m_bootstrap)
        m_L = 30;
      else
        m_L = 3 + NTL::NumBits(m_bitwidth + 2);
    }
    /// initialize HElib context
    m_helib_context = new FHEcontext(m_m, m_p, 1, gens, ords);
    m_helib_context->bitsPerLevel = m_B;
    /// modify context, add primes to modulus chain
    buildModChain(*m_helib_context, m_L, m_c, 8);

    if (m_bootstrap) {
      m_helib_context->makeBootstrappable(mvec, /*t=*/0,
                                          /*flag=*/false, /*cacheType=DCRT*/ 2);
    }

    // unpack slot encoding
    buildUnpackSlotEncoding(m_unpackSlotEncoding, *(m_helib_context->ea));

    /// create secret key structure
    m_secretKey = new FHESecKey(*m_helib_context);

    m_publicKey = m_secretKey;  //// points to the same place

    /// generate a secret key
    m_secretKey->GenSecKey(m_w);  /// Hamming weight of 128

    addSome1DMatrices(*m_secretKey);
    addFrbMatrices(*m_secretKey);

    /// how big are keys?
    this->m_private_key_size = sizeof(*m_secretKey);
    this->m_public_key_size = sizeof(*m_publicKey);

    if (m_bootstrap) m_secretKey->genRecryptData();

    m_ea = new EncryptedArray(*m_helib_context);

    this->m_nslots = m_ea->size();
  };

  // destructor
  virtual ~ContextHElib() {
    /// delete everything we new-ed in the constructor
    if (m_ea != NULL) delete m_ea;
    if (m_secretKey != NULL) delete m_secretKey;
    if (m_helib_context != NULL) delete m_helib_context;
  };

  virtual Ciphertext encrypt(std::vector<Plaintext> pt) = 0;
  virtual std::vector<Plaintext> decrypt(Ciphertext pt) = 0;

 protected:
  long m_param_set;  // which set of parameters to use (0 to 4).
  long m_p;          //  modulus of plaintext
  long m_B;          // number of bits per level
  long m_L;          // maximum number of homomorphic levels
  long m_w;          // Hamming weight of secret key
  long m_c;          // number of columns in key-switching matrix

  long m_m;
  long m_phim;
  long m_d;
  long m_m1;
  long m_m2;
  long m_m3;
  long m_g1;
  long m_g2;
  long m_g3;
  long m_ord1;
  long m_ord2;
  long m_ord3;
  EncryptedArray* m_ea;
  FHESecKey* m_secretKey;
  const FHEPubKey* m_publicKey;
  FHEcontext* m_helib_context;
  int m_bitwidth;
  std::vector<zzX> m_unpackSlotEncoding;
  bool m_bootstrap;
  long m_bootstrapl;  /// long version of the bootstrap flag to allow it to be
                      /// settable from param_name_map

};  //// end of ContextHElib class definition.

////////////////////////////////////////////////////////////////////////////////////
///  ContextHElib_F2 -  use p=2, do everything with arrays of Ciphertext,
///  and binary operations for add, multiply, compare etc.
template <typename PlaintextT>
class ContextHElib_F2 : public ContextHElib<PlaintextT, NTL::Vec<Ctxt> > {
 public:
  typedef PlaintextT Plaintext;
  typedef NTL::Vec<Ctxt> Ciphertext;

  ContextHElib_F2(
      long param_set = 0,         // parameter set, from 0 (tiny) to 4 (huge)
      bool bootstrap = true,      // bootstrap or not?
      long hamming_weight = 128)  // Hamming weight of secret key
      : ContextHElib<Plaintext, Ciphertext>(2, param_set, bootstrap,
                                            hamming_weight) {
    /// this is not nice, but for Compare, it helps to know if we are dealing
    /// with signed or unsigned inputs
    m_signed_plaintext = (std::is_same<Plaintext, int8_t>::value ||
                          std::is_same<Plaintext, int16_t>::value ||
                          std::is_same<Plaintext, int32_t>::value);

    m_bool_plaintext = std::is_same<Plaintext, bool>::value;
  }

  Ciphertext encrypt(std::vector<Plaintext> pt) {
    if (pt.size() > this->m_nslots) {
      throw std::runtime_error(
          "The number of slots is greater than the number of SIMD operations "
          "that can be done at a time");
    }

    Ctxt mu(*(this->m_publicKey), (long)this->m_nslots);
    Ciphertext ct;

    resize(ct, this->m_bitwidth, mu);

    for (int j = 0; j < this->m_bitwidth; j++) {
      vector<ZZX> sliced_pt = vector<ZZX>(this->m_nslots);

      // sliced_pt[i] is the jth bit of input i
      for (int i = 0; i < pt.size(); i++) {
        sliced_pt[i] = ZZX((pt[i] >> j) & 1);
      }

      this->m_ea->encrypt(ct[j], *(this->m_publicKey), sliced_pt);
    }
    return ct;
  }

  std::vector<Plaintext> decrypt(Ciphertext ct) {
    std::vector<long> ct_decrypt = std::vector<long>(this->m_nslots);
    std::vector<Plaintext> pt_transformed =
        std::vector<Plaintext>(this->m_nslots);

    decryptBinaryNums(ct_decrypt, CtPtrs_VecCt(ct), *(this->m_secretKey),
                      *(this->m_ea));

    for (int i = 0; i < this->m_nslots; i++) {
      pt_transformed[i] = ct_decrypt[i] % int(pow(2, this->m_bitwidth));
    }
    return pt_transformed;
  }

  std::string encrypt_and_serialize(std::vector<PlaintextT> pt) {
    Ciphertext ct = encrypt(pt);
    std::stringstream ss;
    for (int i=0; i< this->m_bitwidth; ++i) {
      ss << ct[i];
    }
    std::string ctstring = ss.str();
    return ctstring;
  };

  Ciphertext Negate(Ciphertext a) {
    /// bootstrapping method copied from HElib's Test_binaryCompare
    if (this->m_bootstrap) {
      for (int i = 0; i < this->m_bitwidth; ++i) {
        a[i].modDownToLevel(5);
      }
    }

    /// Two's complement negation - negate all bits then add one
    Ciphertext output;

    for (int i = 0; i < this->m_bitwidth; ++i) {
      Ctxt abit = a[i];
      abit.addConstant(to_ZZX(1L));
      output.append(abit);
    }

    if (this->m_bitwidth == 1)
      return output;  // for a bool, we are already done..

    /// for integers, need to add 1.
    std::vector<Plaintext> one;
    for (int i = 0; i < this->m_nslots; i++) {
      one.push_back(1);
    }

    Ciphertext one_enc = encrypt(one);
    Ciphertext output_final = Add(output, one_enc);

    return output_final;
  }

  Ciphertext Maximum(Ciphertext a, Ciphertext b) {
    /// "Maximum" i.e. "OR" only valid for bool inputs.  If not, call the
    /// base-class function (which will throw a GateNotImplemented error).
    if (!this->m_bool_plaintext) Context<Plaintext, Ciphertext>::Maximum(a, b);
    /// OR(a,b) = XOR( XOR(a,b), AND(a,b))
    Ciphertext output;

    Ctxt a1 = a[0];
    Ctxt a2 = a[0];
    a1 += b[0];  // XOR(a,b)
    a2 *= b[0];  // AND(a,b)
    a1 += a2;    // XOR the previous two lines
    output.append(a1);
    return output;
  }

  Ciphertext Compare_unsigned(Ciphertext a, Ciphertext b) {
    Ctxt mu(*(this->m_publicKey));
    Ctxt ni(*(this->m_publicKey));
    Ciphertext cmax, cmin;
    CtPtrs_VecCt wMin(cmin), wMax(cmax);  /// wrappers around output vectors
    compareTwoNumbers(wMax, wMin, mu, ni, CtPtrs_VecCt(a), CtPtrs_VecCt(b),
                      &(this->m_unpackSlotEncoding));
    /// mu is now a Ctxt which is the encryption of 1 if a>b and 0 otherwise.
    /// but we need to put it into NTL::Vec<Ctxt> as that is our new
    /// "Ciphertext" type.
    Ciphertext output;
    output.append(mu);
    return output;
  }

  Ciphertext Compare_signed(Ciphertext a, Ciphertext b) {
    //// subtract a-b and look at sign-bit
    Ciphertext b_minus_a = Subtract(b, a);
    Ciphertext output;

    Ctxt sign_bit =
        b_minus_a[this->m_bitwidth - 1];  /// is sign-bit set?  if yes, b
    output.append(sign_bit);
    return output;
  }

  Ciphertext Compare(Ciphertext a, Ciphertext b) {
    if (this->m_bootstrap) {
      for (int i = 0; i < this->m_bitwidth; ++i) {
        a[i].modDownToLevel(5);
        b[i].modDownToLevel(5);
      }
    }
    if (this->m_signed_plaintext)
      return Compare_signed(a, b);
    else
      return Compare_unsigned(a, b);
  }

  Ciphertext Subtract(Ciphertext a, Ciphertext b) {
    if (this->m_bitwidth == 1)
      return Add(a, b);  //// for bools, add and subtract are the same

    if (this->m_bootstrap) {
      for (int i = 0; i < this->m_bitwidth; ++i) {
        a[i].modDownToLevel(5);
      }
    }

    Ciphertext output;
    Ciphertext b_neg = Negate(b);
    CtPtrs_VecCt wout(output);
    addTwoNumbers(wout, CtPtrs_VecCt(a), CtPtrs_VecCt(b_neg), this->m_bitwidth,
                  &(this->m_unpackSlotEncoding));
    return output;
  }

  Ciphertext Add(Ciphertext a, Ciphertext b) {
    if (this->m_bootstrap) {
      for (int i = 0; i < this->m_bitwidth; ++i) {
        a[i].modDownToLevel(5);
      }
    }

    Ciphertext sum;
    CtPtrs_VecCt wsum(sum);

    addTwoNumbers(wsum, CtPtrs_VecCt(a), CtPtrs_VecCt(b), this->m_bitwidth,
                  &(this->m_unpackSlotEncoding));

    return sum;
  }

  Ciphertext Multiply(Ciphertext a, Ciphertext b) {
    if (this->m_bootstrap) {
      for (int i = 0; i < this->m_bitwidth; ++i) {
        a[i].modDownToLevel(5);
      }
    }

    Ciphertext product;
    CtPtrs_VecCt wprod(product);

    multTwoNumbers(wprod, CtPtrs_VecCt(a), CtPtrs_VecCt(b), false,
                   this->m_bitwidth, &(this->m_unpackSlotEncoding));

    return product;
  }

  Ciphertext Select(Ciphertext s, Ciphertext a, Ciphertext b) {
    if (this->m_bootstrap) {
      for (int i = 0; i < this->m_bitwidth; ++i) {
        a[i].modDownToLevel(5);
        b[i].modDownToLevel(5);
      }
    }
    /// s is 0 or 1
    /// for each bit of a,b,output, do output = s*a + (1-s)*b
    Ciphertext output;

    for (int i = 0; i < this->m_bitwidth; ++i) {
      Ctxt sbit = s[0];
      Ctxt abit = a[i];
      Ctxt bbit = b[i];
      abit *= sbit;
      sbit.addConstant(to_ZZX(-1L));
      sbit.multByConstant(to_ZZX(-1L));
      sbit *= bbit;
      abit += sbit;
      output.append(abit);
    }
    return output;
  }

  Ciphertext Rotate(Ciphertext a, long n) {
    /// Cyclically rotate the linear array by n positions
    if (n > this->m_nslots) {
      throw std::runtime_error("Error in Rotate: cannot rotate by more than nslots positions");
    }
    Ciphertext result;
    if (n > 0) n = n - this->m_ninputs;
    /// loop over all bits
    for (int j = 0; j < this->m_bitwidth; j++) {
      Ctxt result_bit(a[j]);
      this->m_ea->rotate(result_bit, n);
      result.append(result_bit);
    }
    return result;

  }

 private:
  bool m_signed_plaintext;
  bool m_bool_plaintext;

};  /// end of class definition

// (dummy) specializations for double and complex<double>
template <>
class ContextHElib_F2<double> : public Context<double, NTL::Vec<Ctxt> > {
 public:
  typedef double Plaintext;
  typedef NTL::Vec<Ctxt>  Ciphertext;

  ContextHElib_F2() {
    throw InputTypeNotSupported();
  }
  Ciphertext encrypt(std::vector<Plaintext> pt) {
    throw InputTypeNotSupported();
  }
  std::vector<Plaintext> decrypt(Ciphertext ct) {
    throw InputTypeNotSupported();
  }
};

template <>
class ContextHElib_F2<std::complex<double> >: public Context<std::complex<double>, NTL::Vec<Ctxt> > {
 public:
  typedef std::complex<double> Plaintext;
  typedef NTL::Vec<Ctxt>  Ciphertext;

  ContextHElib_F2() {
    throw InputTypeNotSupported();
  }
  Ciphertext encrypt(std::vector<Plaintext> pt) {
    throw InputTypeNotSupported();
  }
  std::vector<Plaintext> decrypt(Ciphertext ct) {
    throw InputTypeNotSupported();
  }
};

////////////////////////////////////////////////////////////////////////////
//// ContextHElib_Fp  - use integer plaintext space, e.g. p=65537

template <typename PlaintextT>
class ContextHElib_Fp : public ContextHElib<PlaintextT, Ctxt> {
 public:
  typedef PlaintextT Plaintext;
  typedef Ctxt Ciphertext;

  ContextHElib_Fp(
      long p = 65537,             // plaintext modulus
      long param_set = 0,         // parameter set, from 0 (tiny) to 4 (huge)
      bool bootstrap = false,     // bootstrap or not?
      long hamming_weight = 128)  // Hamming weight of secret key
      : ContextHElib<Plaintext, Ciphertext>(p, param_set, bootstrap,
                                            hamming_weight) {
    this->m_ninputs = 0;
    this->m_param_name_map.insert({"p", this->m_p});
  }

  Ciphertext encrypt(std::vector<Plaintext> pt) {
    std::vector<long> ptvec;

    int pt_len = pt.size();

    // Check whether the input is too long to be encrypted in one go
    if (pt_len > this->m_nslots) {
      throw std::runtime_error(
          "The number of slots is greater than the number of SIMD operations "
          "that can be done at a time");
    }
    // convert plaintext input into a vector of longs
    for (int i = 0; i < this->m_nslots; i++) {
      ptvec.push_back(pt[i % pt_len]);
    }

    // encrypt vector of longs
    Ciphertext ct(*(this->m_publicKey));

    this->m_ea->encrypt(ct, *(this->m_publicKey), ptvec);
    this->m_ciphertext_size = sizeof(ct);

    return ct;
  }

  std::vector<Plaintext> decrypt(Ciphertext ct) {
    std::vector<Plaintext> result;
    std::vector<long> pt;

    long pt_transformed;

    this->m_ea->decrypt(ct, *(this->m_secretKey), pt);

    for (int i = 0; i < pt.size(); i++) {
      // convention - treat this as a negative number
      if ((pt[i]) > this->m_p / 2)
        pt_transformed = pt[i] - this->m_p;
      else
        pt_transformed = pt[i];

      result.push_back(pt_transformed % int(pow(2, this->m_bitwidth)));
    }

    return result;
  }

  std::string encrypt_and_serialize(std::vector<PlaintextT> pt) {
    Ciphertext ct = encrypt(pt);
    std::stringstream ss;
    ss << ct;
    std::string ctstring = ss.str();
    return ctstring;
  };

  Ciphertext Add(Ciphertext a, Ciphertext b) {
    a += b;
    return a;
  }

  Ciphertext Subtract(Ciphertext a, Ciphertext b) {
    a -= b;
    return a;
  }

  Ciphertext Multiply(Ciphertext a, Ciphertext b) {
    a *= b;
    return a;
  }

  Ciphertext Negate(Ciphertext a) {
    if (this->m_bitwidth == 1)  /// special case for binary
      a.addConstant(to_ZZX(1L));
    else
      a.multByConstant(to_ZZX(-1L));

    return a;
  }

  Ciphertext MultByConstant(Ciphertext a, long b) {
    a.multByConstant(to_ZZX(b));
    return a;
  }

  Ciphertext AddConstant(Ciphertext a, long b) {
    a.addConstant(to_ZZX(b));

    return a;
  }

  Ciphertext Select(Ciphertext s, Ciphertext a, Ciphertext b) {
    /// s is 0 or 1
    /// output is s*a + (1-s)*b
    Ciphertext sa = Multiply(s, a);
    Ciphertext one_minus_s = MultByConstant(AddConstant(s, -1L), -1L);
    Ciphertext one_minus_s_times_b = Multiply(one_minus_s, b);
    return Add(sa, one_minus_s_times_b);
  }

  Ciphertext Rotate(Ciphertext a, long n) {
    /// Cyclically rotate the linear array by n positions
    if (n > this->m_nslots) {
      throw std::runtime_error("Error in Rotate: cannot rotate by more than nslots positions");
    }
    //to rotate right, we actually rotate left
    // by (ninputs - n) positions
    if (n > 0) n = n - this->m_ninputs;

    Ciphertext result(a);
    this->m_ea->rotate(result, n);

    return result;
  }

};  /// end of class definition

// (dummy) specializations for double and complex<double>
template <>
class ContextHElib_Fp<double> : public Context<double, Ctxt > {
 public:
  typedef double Plaintext;
  typedef Ctxt  Ciphertext;

  ContextHElib_Fp() {
    throw InputTypeNotSupported();
  }
  Ciphertext encrypt(std::vector<Plaintext> pt) {
    throw InputTypeNotSupported();
  }
  std::vector<Plaintext> decrypt(Ciphertext ct) {
    throw InputTypeNotSupported();
  }
};

template <>
class ContextHElib_Fp<std::complex<double> >: public Context<std::complex<double>, Ctxt > {
 public:
  typedef std::complex<double> Plaintext;
  typedef Ctxt  Ciphertext;

  ContextHElib_Fp() {
    throw InputTypeNotSupported();
  }
  Ciphertext encrypt(std::vector<Plaintext> pt) {
    throw InputTypeNotSupported();
  }
  std::vector<Plaintext> decrypt(Ciphertext ct) {
    throw InputTypeNotSupported();
  }
};

}  // namespace SHEEP

#endif  // CONTEXT_HELIB_HPP


/*
*  Copyright 2012 Matthias Fuchs
*
*  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 <boost/assert.hpp>
#include <boost/functional/hash.hpp>
#include <boost/graph/adjacency_matrix.hpp>
#include <boost/graph/topological_sort.hpp>
#include <iostream>
#include "stromx/runtime/Input.h"
#include "stromx/runtime/InputConnector.h"
#include "stromx/runtime/Operator.h"
#include "stromx/runtime/SortInputsAlgorithm.h"
#include "stromx/runtime/Stream.h"
#include "stromx/runtime/Thread.h"

namespace stromx
{
    namespace runtime
    {
        namespace
        {
            class InputComparator
            {
        public:
                InputComparator(const std::map<const Operator*, unsigned int> & op2RankMap)
                  : m_op2RankMap(op2RankMap)
                {}

                bool operator()(const InputConnector & input1, const InputConnector & input2)
                {

                    std::map<const Operator*, unsigned int>::const_iterator rank1 = m_op2RankMap.find(input1.op());
                    BOOST_ASSERT(rank1 != m_op2RankMap.end());

                    std::map<const Operator*, unsigned int>::const_iterator rank2 = m_op2RankMap.find(input2.op());
                    BOOST_ASSERT(rank2 != m_op2RankMap.end());

                    return rank1->second > rank2->second ? true : false;
                }

            private:
                const std::map<const Operator*, unsigned int> & m_op2RankMap;
            };
        }

        void SortInputsAlgorithm::apply ( Stream& stream )
        {
            const std::vector<Operator*> & operators = stream.initializedOperators();
            unsigned int numNodes = operators.size();

            if(! numNodes)
                return;

            std::map<const Operator*, unsigned int> op2IdMap;
            std::map<unsigned int, const Operator*> id2OpMap;

            // fill the maps
            unsigned int i = 0;
            for(std::vector<Operator*>::const_iterator opIter = operators.begin();
                opIter != operators.end();
                ++opIter, ++i)
            {
                op2IdMap[*opIter] = i;
                id2OpMap[i] = *opIter;
            }

            // create the graph
            boost::adjacency_matrix<boost::directedS> graph(numNodes);

            // iterate over each operator
            for(std::vector<Operator*>::const_iterator opIter = operators.begin();
                opIter != operators.end();
                ++opIter)
            {
                const std::vector<const Input*> & inputs = (*opIter)->info().inputs();
                for(std::vector<const Input*>::const_iterator inputIter = inputs.begin();
                    inputIter != inputs.end();
                    ++inputIter)
                {
                    OutputConnector output = stream.connectionSource(*opIter, (*inputIter)->id());
                    if(output.valid())
                    {
                        const Operator* source = output.op();
                        const Operator* target = *opIter;

                        boost::add_edge(op2IdMap[source], op2IdMap[target], graph);
                    }
                }
            }

            // allocate a vector for the output
            std::vector<unsigned int> output(operators.size());
            std::vector<unsigned int>::iterator outputIter = output.begin();

            // run the algorithm
            boost::topological_sort(graph, outputIter);

            // map the operators to their ranks
            std::map<const Operator*, unsigned int> op2RankMap;
            for(unsigned int rank = 0; rank < output.size(); ++rank)
            {
                op2RankMap[id2OpMap[output[rank]]] = rank;
            }

            // reorder the input sequences
            const std::vector<Thread*> & threads = stream.threads();
            for(std::vector<Thread*>::const_iterator threadIter = threads.begin();
                threadIter != threads.end();
                ++threadIter)
            {
                Thread* thread = *threadIter;
                std::vector<InputConnector> inputs = thread->inputSequence();

                // delete all inputs
                while(thread->inputSequence().size())
                    thread->removeInput(thread->inputSequence().size() - 1);

                InputComparator comp(op2RankMap);
                std::sort(inputs.begin(), inputs.end(), comp);

                for(std::vector<InputConnector>::const_iterator inputIter = inputs.begin();
                    inputIter != inputs.end();
                    ++inputIter)
                {
                    // add the input to the thread
                    // cast away constness of the operator (non-const access is anyways
                    // available via Stream::operators())
                    thread->addInput(const_cast<Operator*>(inputIter->op()), inputIter->id());
                }
            }
        }
    }
}


//==============================================================================
//         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_OPERATOR_FUNCTIONS_SCALAR_MULTIPLIES_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_OPERATOR_FUNCTIONS_SCALAR_MULTIPLIES_HPP_INCLUDED

#include <boost/simd/toolbox/operator/functions/multiplies.hpp>
#include <boost/mpl/times.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, tag::cpu_, (A0)
                            , (scalar_< arithmetic_<A0> >)
                              (scalar_< arithmetic_<A0> >)
                            )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return a0 * a1; }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, tag::cpu_, (A0)(A1)
                            , (mpl_integral_< scalar_< arithmetic_<A0> > >)
                              (mpl_integral_< scalar_< arithmetic_<A1> > >)
                            )
  {
    typedef typename mpl::times<A0, A1>::type result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const&, A1 const&) const
    {
      return result_type();
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, tag::cpu_, (A0)(A1)
                            , (scalar_< arithmetic_<A0> >)
                              (mpl_integral_< scalar_< arithmetic_<A1> > >)
                            )
  {
    typedef A0 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const& a0, A1 const&) const
    {
      return a0*A1::value;
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION( boost::simd::tag::multiplies_, tag::cpu_, (A0)(A1)
                            , (mpl_integral_< scalar_< arithmetic_<A0> > >)
                              (scalar_< arithmetic_<A1> >)
                            )
  {
    typedef A1 result_type;
    BOOST_FORCEINLINE result_type operator()(A0 const&, A1 const& a1) const
    {
      return a1*A0::value;
    }
  };
} } }

#endif


#include <iostream>
#include <stdexcept>
#include <Eigen/Dense>
#include <Eigen/SparseCore>
#include "learnreg.h"
#include "mexutils.h"

using namespace Eigen;

/***** MATLAB mex function *****/
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    //mexPrintf("DEBUG: Before loading mex input");
    /* Verify and load arguments */
    Eigen::SparseMatrix<double> X;
    Eigen::VectorXd Y;
    double lam, nodeBeta = 0;
    Eigen::VectorXd phi;
    try {
        if( nrhs < 3 ) { throw std::invalid_argument( "Too few arguments.  Need at least 3." ); }
        X = getSparseMatrix( prhs[0] );
        Y = getVector( prhs[1] );
        lam = getScalar<double>( prhs[2] );
        if(nrhs >= 4) {
            nodeBeta = getScalar<double>( prhs[3] );
        }
        if(nrhs >= 5) {
            phi = getVector( prhs[4] );
        }
    } catch ( const std::invalid_argument& e ) {
        std::cerr << "Error loading arguments: " << e.what() << std::endl;
        fake_answer(plhs);
        return;
    }
    mexPrintf("DEBUG: After loading mex input, nodeBeta = %g", nodeBeta);

    /* Simply call ais function */
    //mexPrintf("DEBUG: Before calling ais function");
    GeneralizedLinearModel<Poisson> poisson;
    poisson.learnreg( X, Y, lam, nodeBeta, phi );
    //mexPrintf("DEBUG: After calling ais function");

    /* Save final answer into MATLAB output */
    //mexPrintf("DEBUG: Before creating mex output arguments");
    if( nlhs >= 1 ) {
        plhs[0] = eigenVec2matlabVec( phi );
    }
    //mexPrintf("DEBUG: After creating mex output arguments");
}


#ifndef GAME_COMPILE
#include <iostream>
#include <boost/random/random_device.hpp>
#include <boost/filesystem.hpp>
#include "FileManager.h"
#include "RoomPlacer.h"
#include "Pattern.h"
#include "WaveFunctionCollapse.h"
#include "DRandom.h"

int main(int argc, char* argv[])
{
	if (argc == 3)
	{
		boost::random::random_device device;
		uint32_t seed = device();

		boost::filesystem::path tex_path = argv[1];
		boost::filesystem::path json_path = boost::filesystem::change_extension(tex_path, "json");

		int n = std::stoi(argv[2]);
		WFC::Pattern::ImageToJSON(tex_path.string(), json_path.string(), n);

		DRandom rand(seed);
		WFC::WaveFunctionCollapse wfc(json_path.string(), 64, 64);
		wfc.RunVisualiseOnly();
	}
	else if (argc == 1)
	{
		FileManager::ConvertPatternsToJSON(true);
		FileManager::ConvertRoomsToJSON(true);

		while (true)
		{
			boost::random::random_device device;
			uint32_t seed = device();
			//uint32_t seed = 3082447256;
			std::cout << "Seed: " << seed << "\n";

			RoomPlacer rp("C:\\Users\\Example\\Documents\\git\\SpaghettiDungeonGen\\");
			Requirements requirements;
			requirements.min_rooms = 30;
			requirements.min_boss_distance = 4;
			requirements.min_treasure_distance = 3;
			requirements.min_shop_distance = 2;
			requirements.min_non_start_distance = 2;
			rp.PlaceRooms(120, 120, seed, requirements);
		}
	}
	else
	{
		std::cout << "Unknown number of arguments" << std::endl;
	}

	return 0;
}
#endif


//  (C) Copyright John Maddock 2005.
//  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_TR1_COMPLEX_HPP_INCLUDED
#  define BOOST_TR1_COMPLEX_HPP_INCLUDED
#  include <boost/tr1/detail/config.hpp>
#  include <complex>

#ifndef BOOST_HAS_TR1_COMPLEX_OVERLOADS

#include <boost/tr1/detail/math_overloads.hpp>
#include <boost/assert.hpp>
#include <boost/detail/workaround.hpp>
#include <cmath>

namespace std{

#ifdef BOOST_NO_STDC_NAMESPACE
   using :: atan2;
#endif

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
template <class T>
inline BOOST_TR1_MATH_RETURN(double) arg(const T& t)
{
   return ::std::atan2(0.0, static_cast<double>(t));
}
#else
inline double arg(const double& t)
{
   return ::std::atan2(0.0, t);
}
#endif
inline long double arg(const long double& t)
{
   return ::std::atan2(0.0L, static_cast<long double>(t));
}
inline float arg(const float& t)
{
   return ::std::atan2(0.0F, static_cast<float>(t));
}

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
template <class T>
inline BOOST_TR1_MATH_RETURN(double) norm(const T& t)
{
   double r = static_cast<double>(t);
   return r*r;
}
#else
inline double norm(const double& t)
{
   return t*t;
}
#endif
inline long double norm(const long double& t)
{
   long double l = t;
   return l*l;
}
inline float norm(const float& t)
{
   float f = t;
   return f*f;
}

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
template <class T>
inline BOOST_TR1_MATH_RETURN(std::complex<double>) conj(const T& t)
{
   return ::std::conj(std::complex<double>(static_cast<double>(t)));
}
#else
inline std::complex<double> conj(const double& t)
{
   return ::std::conj(std::complex<double>(t));
}
#endif
inline std::complex<long double> conj(const long double& t)
{
   return ::std::conj(std::complex<long double>(t));
}
inline std::complex<float> conj(const float& t)
{
   std::complex<float> ct(t);
   ct = ::std::conj(ct);
   return ct;
}

#if !BOOST_WORKAROUND(__BORLANDC__, <=0x570) && !BOOST_WORKAROUND(BOOST_MSVC, < 1310)
inline complex<double> polar(const char& rho, const char& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const unsigned char& rho, const unsigned char& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const signed char& rho, const signed char& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const short& rho, const short& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const unsigned short& rho, const unsigned short& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const int& rho, const int& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const unsigned int& rho, const unsigned int& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const long& rho, const long& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const unsigned long& rho, const unsigned long& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
#ifdef BOOST_HAS_LONG_LONG
inline complex<double> polar(const long long& rho, const long long& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const unsigned long long& rho, const unsigned long long& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
#elif defined(BOOST_HAS_MS_INT64)
inline complex<double> polar(const __int64& rho, const __int64& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
inline complex<double> polar(const unsigned __int64& rho, const unsigned __int64& theta = 0)
{ return ::std::polar(static_cast<double>(rho), static_cast<double>(theta)); }
#endif

template<class T, class U>
inline complex<typename boost::tr1_detail::promote_to_real<T, U>::type>
   polar(const T& rho, const U& theta)
{
   typedef typename boost::tr1_detail::promote_to_real<T, U>::type real_type;
   return std::polar(static_cast<real_type>(rho), static_cast<real_type>(theta));
}
#endif

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
template <class T>
inline BOOST_TR1_MATH_RETURN(double) imag(const T& )
{
   return 0;
}
#else
inline double imag(const double& )
{
   return 0;
}
#endif
inline long double imag(const long double& )
{
   return 0;
}
inline float imag(const float& )
{
   return 0;
}

#ifndef BOOST_NO_FUNCTION_TEMPLATE_ORDERING
template <class T>
inline BOOST_TR1_MATH_RETURN(double) real(const T& t)
{
   return static_cast<double>(t);
}
#else
inline double real(const double& t)
{
   return t;
}
#endif
inline long double real(const long double& t)
{
   return t;
}
inline float real(const float& t)
{
   return t;
}

template<class T, class U>
inline complex<typename boost::tr1_detail::largest_real<T, U>::type>
   pow(const complex<T>& x, const complex<U>& y)
{
   typedef complex<typename boost::tr1_detail::largest_real<T, U>::type> result_type;
   typedef typename boost::mpl::if_<boost::is_same<result_type, complex<T> >, result_type const&, result_type>::type cast1_type;
   typedef typename boost::mpl::if_<boost::is_same<result_type, complex<U> >, result_type const&, result_type>::type cast2_type;
   cast1_type x1(x);
   cast2_type y1(y);
   return std::pow(x1, y1);
}
template<class T, class U>
inline complex<typename boost::tr1_detail::promote_to_real<T, U>::type>
   pow (const complex<T>& x, const U& y)
{
   typedef typename boost::tr1_detail::promote_to_real<T, U>::type real_type;
   typedef complex<typename boost::tr1_detail::promote_to_real<T, U>::type> result_type;
   typedef typename boost::mpl::if_<boost::is_same<result_type, complex<T> >, result_type const&, result_type>::type cast1_type;
   real_type r = y;
   cast1_type x1(x);
   std::complex<real_type> y1(r);
   return std::pow(x1, y1);
}

template<class T, class U>
inline complex<typename boost::tr1_detail::promote_to_real<T, U>::type>
   pow (const T& x, const complex<U>& y)
{
   typedef typename boost::tr1_detail::promote_to_real<T, U>::type real_type;
   typedef complex<typename boost::tr1_detail::promote_to_real<T, U>::type> result_type;
   typedef typename boost::mpl::if_<boost::is_same<result_type, complex<U> >, result_type const&, result_type>::type cast_type;
   real_type r = x;
   std::complex<real_type> x1(r);
   cast_type y1(y);
   return std::pow(x1, y1);
}

}

#endif

#ifndef BOOST_HAS_TR1_COMPLEX_INVERSE_TRIG

#include <boost/math/complex.hpp>

namespace std {
namespace tr1 {

using boost::math::acos;
using boost::math::asin;
using boost::math::atan;
using boost::math::acosh;
using boost::math::asinh;
using boost::math::atanh;
using boost::math::fabs;

} }

#else

#  ifdef BOOST_HAS_INCLUDE_NEXT
#     include_next BOOST_TR1_HEADER(complex)
#  else
#     include <boost/tr1/detail/config_all.hpp>
#     include BOOST_TR1_STD_HEADER(BOOST_TR1_PATH(complex))
#  endif

#endif

#endif


//  Copyright John Maddock 2005-2006.
//  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_MATH_FPCLASSIFY_HPP
#define BOOST_MATH_FPCLASSIFY_HPP

#ifdef _MSC_VER
#pragma once
#endif

#include <math.h>
#include <cmath>
#include <boost/limits.hpp>
#include <boost/math/tools/real_cast.hpp>
#include <boost/type_traits/is_floating_point.hpp>
#include <boost/math/special_functions/math_fwd.hpp>

#if defined(_MSC_VER) || defined(__BORLANDC__)
#include <float.h>
#endif

#ifdef BOOST_NO_STDC_NAMESPACE
  namespace std{ using ::abs; using ::fabs; }
#endif

#ifndef FP_NORMAL

#define FP_ZERO        0
#define FP_NORMAL      1
#define FP_INFINITE    2
#define FP_NAN         3
#define FP_SUBNORMAL   4

#else

#define BOOST_HAS_FPCLASSIFY

#ifndef fpclassify
#  if (defined(__GLIBCPP__) || defined(__GLIBCXX__)) \
         && defined(_GLIBCXX_USE_C99_MATH) \
         && !(defined(_GLIBCXX_USE_C99_FP_MACROS_DYNAMIC) \
         && (_GLIBCXX_USE_C99_FP_MACROS_DYNAMIC != 0))
#     ifdef _STLP_VENDOR_CSTD
#        define BOOST_FPCLASSIFY_PREFIX ::_STLP_VENDOR_CSTD::
#     else
#        define BOOST_FPCLASSIFY_PREFIX ::std::
#     endif
#  else
#     undef BOOST_HAS_FPCLASSIFY
#     define BOOST_FPCLASSIFY_PREFIX
#  endif
#elif (defined(__HP_aCC) && !defined(__hppa))
// aCC 6 appears to do "#define fpclassify fpclassify" which messes us up a bit!
#  define BOOST_FPCLASSIFY_PREFIX ::
#else
#  define BOOST_FPCLASSIFY_PREFIX
#endif

#ifdef __MINGW32__
#  undef BOOST_HAS_FPCLASSIFY
#endif

#endif

namespace boost{

#if defined(BOOST_HAS_FPCLASSIFY) || defined(isnan)
//
// This must not be located in any namespace under boost::math
// otherwise we can get into an infinite loop if isnan is
// a #define for "isnan" !
//
namespace math_detail{

template <class T>
inline bool is_nan_helper(T t, const boost::true_type&)
{
#ifdef isnan
   return isnan(t);
#else // BOOST_HAS_FPCLASSIFY
   return (BOOST_FPCLASSIFY_PREFIX fpclassify(t) == (int)FP_NAN);
#endif
}

template <class T>
inline bool is_nan_helper(T t, const boost::false_type&)
{
   return false;
}

}

#endif // defined(BOOST_HAS_FPCLASSIFY) || defined(isnan)

namespace math{

namespace detail{

template <class T>
inline int fpclassify_imp BOOST_NO_MACRO_EXPAND(T t, const mpl::true_&)
{
   // whenever possible check for Nan's first:
#ifdef BOOST_HAS_FPCLASSIFY
   if(::boost::math_detail::is_nan_helper(t, ::boost::is_floating_point<T>()))
      return FP_NAN;
#elif defined(isnan)
   if(boost::math_detail::is_nan_helper(t, ::boost::is_floating_point<T>()))
      return FP_NAN;
#elif defined(_MSC_VER) || defined(__BORLANDC__)
   if(::_isnan(boost::math::tools::real_cast<double>(t)))
      return FP_NAN;
#endif
   // std::fabs broken on a few systems especially for long long!!!!
   T at = (t < T(0)) ? -t : t;

   // Use a process of exclusion to figure out
   // what kind of type we have, this relies on
   // IEEE conforming reals that will treat
   // Nan's as unordered.  Some compilers
   // don't do this once optimisations are
   // turned on, hence the check for nan's above.
   if(at <= (std::numeric_limits<T>::max)())
   {
      if(at >= (std::numeric_limits<T>::min)())
         return FP_NORMAL;
      return (at != 0) ? FP_SUBNORMAL : FP_ZERO;
   }
   else if(at > (std::numeric_limits<T>::max)())
      return FP_INFINITE;
   return FP_NAN;
}

template <class T>
inline int fpclassify_imp BOOST_NO_MACRO_EXPAND(T t, const mpl::false_&)
{
   //
   // An unknown type with no numeric_limits support,
   // so what are we supposed to do we do here?
   //
   return t == 0 ? FP_ZERO : FP_NORMAL;
}

}  // namespace detail

template <class T>
inline int fpclassify BOOST_NO_MACRO_EXPAND(T t)
{
#ifdef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
   if(std::numeric_limits<T>::is_specialized)
      return detail::fpclassify_imp(t, mpl::true_());
   return detail::fpclassify_imp(t, mpl::false_());
#else
   return detail::fpclassify_imp(t, mpl::bool_< ::std::numeric_limits<T>::is_specialized>());
#endif
}

#if defined(BOOST_HAS_FPCLASSIFY)
inline int fpclassify BOOST_NO_MACRO_EXPAND(float t)
{
   return BOOST_FPCLASSIFY_PREFIX fpclassify(t);
}
inline int fpclassify BOOST_NO_MACRO_EXPAND(double t)
{
   return BOOST_FPCLASSIFY_PREFIX fpclassify(t);
}
#if !defined(__CYGWIN__) && !defined(__HP_aCC) && !defined(BOOST_INTEL) && !defined(BOOST_NO_NATIVE_LONG_DOUBLE_FP_CLASSIFY)
// The native fpclassify broken for long doubles with aCC
// use portable one instead....
inline int fpclassify BOOST_NO_MACRO_EXPAND(long double t)
{
   return BOOST_FPCLASSIFY_PREFIX fpclassify(t);
}
#endif

#elif defined(_MSC_VER)
// This only works for type double, for both float
// and long double it gives misleading answers.
inline int fpclassify BOOST_NO_MACRO_EXPAND(double t)
{
   switch(::_fpclass(t))
   {
   case _FPCLASS_SNAN /* Signaling NaN */ :
   case _FPCLASS_QNAN /* Quiet NaN */ :
      return FP_NAN;
   case _FPCLASS_NINF /*Negative infinity ( -INF) */ :
   case _FPCLASS_PINF /* Positive infinity (+INF) */ :
      return FP_INFINITE;
   case _FPCLASS_NN /* Negative normalized non-zero */ :
   case _FPCLASS_PN /* Positive normalized non-zero */ :
      return FP_NORMAL;
   case _FPCLASS_ND /* Negative denormalized */:
   case _FPCLASS_PD /* Positive denormalized */ :
      return FP_SUBNORMAL;
   case _FPCLASS_NZ /* Negative zero ( - 0) */ :
   case _FPCLASS_PZ /* Positive 0 (+0) */ :
      return FP_ZERO;
   default:
      /**/ ;
   }
   return FP_NAN;  // should never get here!!!
}
#endif

template <class T>
inline bool isfinite BOOST_NO_MACRO_EXPAND(T z)
{
   int t = (::boost::math::fpclassify)(z);
   return (t != (int)FP_NAN) && (t != (int)FP_INFINITE);
}

template <class T>
inline bool isinf BOOST_NO_MACRO_EXPAND(T t)
{
   return (::boost::math::fpclassify)(t) == (int)FP_INFINITE;
}

template <class T>
inline bool isnan BOOST_NO_MACRO_EXPAND(T t)
{
   return (::boost::math::fpclassify)(t) == (int)FP_NAN;
}
#ifdef isnan
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<float>(float t){ return ::boost::math_detail::is_nan_helper(t, boost::true_type()); }
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<double>(double t){ return ::boost::math_detail::is_nan_helper(t, boost::true_type()); }
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<long double>(long double t){ return ::boost::math_detail::is_nan_helper(t, boost::true_type()); }
#elif defined(BOOST_MSVC)
#  pragma warning(push)
#  pragma warning(disable: 4800) // forcing value to bool 'true' or 'false'
#  pragma warning(disable: 4244) // conversion from 'long double' to 'double',
// No possible loss of data because they are same size.
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<float>(float t){ return _isnan(t); }
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<double>(double t){ return _isnan(t); }
template <> inline bool isnan BOOST_NO_MACRO_EXPAND<long double>(long double t){ return _isnan(t); }
#pragma warning (pop)
#endif

template <class T>
inline bool isnormal BOOST_NO_MACRO_EXPAND(T t)
{
   return (::boost::math::fpclassify)(t) == (int)FP_NORMAL;
}

} // namespace math
} // namespace boost

#endif // BOOST_MATH_FPCLASSIFY_HPP


/*
 * TfPublisher.cpp
 *
 *  Created on: Apr 27, 2021
 *      Author: jelavice
 */

#include "icp_localization/transform/TfPublisher.hpp"
#include "icp_localization/transform/FrameTracker.hpp"
#include "icp_localization/transform/ImuTracker.hpp"
#include <eigen_conversions/eigen_msg.h>
#include <Eigen/Dense>
#include "icp_localization/common/assert.hpp"

namespace icp_loco {

namespace {

constexpr bool isDebug = false;

geometry_msgs::TransformStamped toRos(const Rigid3d &T, const Time &time,
		const std::string &frame, const std::string &childFrame) {
	geometry_msgs::TransformStamped transformStamped;
	transformStamped.header.stamp = toRos(time);
	transformStamped.header.frame_id = frame;
	transformStamped.child_frame_id = childFrame;
	tf::vectorEigenToMsg(T.translation(),
			transformStamped.transform.translation);
	tf::quaternionEigenToMsg(T.rotation(), transformStamped.transform.rotation);
	return transformStamped;
}

}  // namespace

TfPublisher::TfPublisher(const ros::NodeHandle &nh,
		std::shared_ptr<FrameTracker> frameTracker,
		std::shared_ptr<ImuTracker> imuTracker) :
		nh_(nh), frameTracker_(frameTracker), imuTracker_(imuTracker), initPosition_(
				Eigen::Vector3d::Zero()), initOrientation_(
				Eigen::Quaterniond::Identity()) {

}

void TfPublisher::setOdometryTopic(const std::string &topic) {
	odometryTopic_ = topic;
}

void TfPublisher::setImuTopic(const std::string &topic) {
	imuTopic_ = topic;
}

void TfPublisher::initialize() {

	odomSubscriber_ = nh_.subscribe(odometryTopic_, 1,
			&TfPublisher::odometryCallback, this);
	imuSubscriber_ = nh_.subscribe(imuTopic_, 1, &TfPublisher::imuCallback,
			this);

}

void TfPublisher::publishMapToRangeSensor(const Time &t) {
	const auto mapToLidar = frameTracker_->getTransformMapToRangeSensor(t);
	geometry_msgs::TransformStamped transformStamped = toRos(mapToLidar, t,
			"map", "range_sensor");
	tfBroadcaster_.sendTransform(transformStamped);
}

void TfPublisher::setIsProvideOdomFrame(bool value) {
	isProvideOdomFrame_ = value;
}

void TfPublisher::setIsUseOdometry(bool value){
	isUseOdometry_ = value;
}

void TfPublisher::setInitialPose(const Eigen::Vector3d &p, const Eigen::Quaterniond &q){
	initPosition_ = p;
	initOrientation_ = q;
}

void TfPublisher::publishMapToOdom(const Time &t) {
	const auto mapToOdom = frameTracker_->getTransformMapToOdom(t);
	geometry_msgs::TransformStamped transformStamped = toRos(mapToOdom, t,
			"map", "odom");
	tfBroadcaster_.sendTransform(transformStamped);
	//just for debug

	if (isDebug) {
		const auto mapToLidar = frameTracker_->getTransformMapToRangeSensor(t);
		transformStamped = toRos(mapToLidar, t, "map", "rs_check");
		tfBroadcaster_.sendTransform(transformStamped);

		const auto lidarToCam =
				(frameTracker_->getTransformOdomSourceToRangeSensor(t)).inverse();
		geometry_msgs::TransformStamped transformStamped = toRos(lidarToCam, t,
				"rs_check", "os_check");
		tfBroadcaster_.sendTransform(transformStamped);

		const auto check = mapToLidar * lidarToCam;
		transformStamped = toRos(check, t, "map", "rs_os_check");
		tfBroadcaster_.sendTransform(transformStamped);
	}

}

void TfPublisher::odometryCallback(const nav_msgs::Odometry &msg) {
	Eigen::Vector3d t;
	Eigen::Quaterniond q;
	tf::quaternionMsgToEigen(msg.pose.pose.orientation, q);
	tf::pointMsgToEigen(msg.pose.pose.position, t);
	TimestampedTransform odomToTracking;
	odomToTracking.transform_ = Rigid3d(t, q);
	odomToTracking.time_ = fromRos(msg.header.stamp);
	frameTracker_->setTransformOdomToOdomSource(odomToTracking);

	if (isProvideOdomFrame_) {
		geometry_msgs::TransformStamped transformStamped = toRos(
				odomToTracking.transform_, odomToTracking.time_, "odom",
				"odom_source");
		tfBroadcaster_.sendTransform(transformStamped);

		const auto cameraToLidar =
				frameTracker_->getTransformOdomSourceToRangeSensor(
						odomToTracking.time_);
		transformStamped = toRos(cameraToLidar,
				odomToTracking.time_, "odom_source", "range_sensor");
		tfBroadcaster_.sendTransform(transformStamped);

		// just visualize raw odometry
		const Rigid3d initPose(initPosition_,initOrientation_);
		const Rigid3d lidarToOdomSource = frameTracker_->getTransformOdomSourceToRangeSensor(Time(fromSeconds(0))).inverse();
		transformStamped = toRos(initPose * lidarToOdomSource,
				odomToTracking.time_, "map", "init_odom_source");
		tfBroadcaster_.sendTransform(transformStamped);
		transformStamped = toRos(
						odomToTracking.transform_, odomToTracking.time_, "init_odom_source",
						"raw_odometry_source");
		tfBroadcaster_.sendTransform(transformStamped);
	}

}

void TfPublisher::imuCallback(const sensor_msgs::Imu &msg) {
	TimestampedImuReading imuReading = fromRos(msg);
	imuTracker_->addReading(imuReading.time_, imuReading.imu_);
//  std::cout <<"Adding imu reading: \n" << imuReading.imu_.asString() << std::endl;
	const auto lidarToImu = frameTracker_->getTransformImuToRangeSensor(
			imuReading.time_).inverse();
	geometry_msgs::TransformStamped transformStamped = toRos(lidarToImu,
			imuReading.time_, "range_sensor", "inertial_sensor");
	tfBroadcaster_.sendTransform(transformStamped);

//	{
//	Rigid3d odomToOdomSource = imuTracker_->getLatestOdometry();
////	std::cout << "IMu odom: " << odomToOdomSource.asString() << std::endl;
//	geometry_msgs::TransformStamped transformStamped = toRos(odomToOdomSource,
//			imuReading.time_, "map", "odom_imu");
//	tfBroadcaster_.sendTransform(transformStamped);
//	}
	//todo I should fuse those two somehow
	if (isProvideOdomFrame_ && !isUseOdometry_) {

	}

//  const auto end = imuTracker_->getBuffer().latest_time();
//  const auto start = end - fromSeconds(1.0);
//  std::cout << "dXYZ: " << imuTracker_->getLienarPositionChange(start, end, Eigen::Vector3d::Ones()).transpose() << std::endl;
//  const auto imuOrientation = imuTracker_->orientation(end);
//  const auto q = imuTracker_->getOrientationChange(start, end) * imuOrientation.inverse();
//  ROS_INFO_STREAM_THROTTLE(1.0, "Imu orientation: " << 180. / M_PI * toRPY(imuOrientation).transpose());
//  std::cout << "d RPY: " << 180. / M_PI * toRPY(q).transpose() << "\n \n";
}

}  // namespace icp_loco