#pragma once
#include "common.hpp"
#include <boost/preprocessor/variadic/to_tuple.hpp>
#include <ph_vari/vari.hpp>
#include <ph_type/type.hpp>

using namespace std;

struct token
{
    enum type
    {
        number,
        plus,
        minus,
        mult,
        div,
        lparen,
        rparen
    };

    friend ostream& operator<< (ostream& os, token const& t)
    {
        return os << "token: " << t.m_text;
    }

    type m_type;
    string m_text;
};

namespace ph {

struct exp
{
//    virtual double eval () = 0;
    exp () = default;
};
#define TOKENS \
    ph::number_t, \
    ph::plus_t, \
    ph::minus_t, \
    ph::multi_t, \
    ph::divi_t, \
    ph::lparen_t, \
    ph::rparen_t, \
    ph::factor_t <ph::number_t>, \
    ph::factor_t <ph::minus_t, ph::number_t>, \
    ph::factor_t <ph::lparen_t, ph::expression_t, ph::rparen_t>, \
    ph::factor_t <ph::minus_t, ph::lparen_t, ph::expression_t, ph::rparen_t>, \
    ph::term_t <ph::factor_t <ph::number_t>>, \
    ph::term_t <ph::factor_t <ph::minus_t, ph::number_t>>, \
    ph::term_t <ph::factor_t <ph::lparen_t, ph::expression_t, ph::rparen_t>>, \
    ph::expression_t

#define MY_LIST \
    X (number_t) \
    X (plus_t) \
    X (minus_t) \
    X (multi_t) \
    X (divi_t) \
    X (lparen_t) \
    X (rparen_t) \
    X (factor_t <number_t>) \
    X (factor_t <minus_t, number_t>) \
    X (factor_t <lparen_t, expression_t, rparen_t>) \
    X (factor_t <minus_t, lparen_t, expression_t, rparen_t>) \
    X (term_t <factor_t <number_t>>) \
    X (term_t <factor_t <minus_t, number_t>>) \
    X (term_t <factor_t <lparen_t, expression_t, rparen_t>>) \
    X (expression_t)

using number_t = double;

struct plus_t
{

};
constexpr plus_t plus {};
struct minus_t
{

};
constexpr minus_t minus {};
struct multi_t
{

};
constexpr multi_t multi {};
struct divi_t
{

};
constexpr divi_t divi {};
struct lparen_t
{

};
constexpr lparen_t lparen {};
struct rparen_t
{

};
constexpr rparen_t rparen {};

//template <typename...>
struct expression_t
{
    explicit expression_t () = default;
};

template <typename...>
struct factor_t;

template <typename...>
struct term_t;

template <>
struct factor_t <void>
{
    virtual void kiss () = 0;
};

template <>
struct factor_t <number_t> : factor_t <void>
{
    number_t m_number;
    factor_t () = default;
    template <typename Variant>
    factor_t (Variant&& vari) : m_number (forward <Variant> (vari))
    {

    }

    virtual void kiss ()
    {
        cout << "kiss" << endl;
    }
};

template <>
struct factor_t <minus_t, number_t> : factor_t <void>
{
    number_t m_number;
    factor_t () = default;

    template <typename... T>
    factor_t (var <T...> const& vari) : m_number (vari)
    {

    }
    factor_t (number_t&& number) : m_number {move (number)}
    {

    }
    factor_t (number_t const& number) : m_number {number}
    {

    }

    virtual void kiss ()
    {
        cout << "kiss" << endl;
    }

//    template <typename... T>
//    operator term_t <T...> () &&
//    {
//        return m_number;
//    }
};

template <>
struct factor_t <lparen_t, expression_t, rparen_t> : factor_t <void>
{
    number_t m_number;
    factor_t () = default;
    template <typename Variant>
    factor_t (Variant&& vari) : m_number (forward <Variant> (vari) * -1)
    {

    }
    virtual void kiss ()
    {
        cout << "kiss" << endl;
    }
};

template <>
struct factor_t <minus_t, lparen_t, expression_t, rparen_t> : factor_t <void>
{
    number_t m_number;

    factor_t () = default;

    template <typename Variant>
    factor_t (Variant&& vari) : m_number (get <number_t> (forward <Variant> (vari)) * -1)
    {

    }

    factor_t (factor_t&& other) : m_number {move (other.m_number)}
    {

    }

    factor_t (factor_t const& other) : m_number {other.m_number}
    {

    }

    factor_t& operator= (factor_t const& other)
    {
        m_number = other.m_number;
        return *this;
    }
    virtual void kiss ()
    {
        cout << "kiss" << endl;
    }
};

template <typename... T>
factor_t <T...> const factor {};

#define FACTORS \
factor_t <number_t>, \
factor_t <minus_t, number_t>, \
factor_t <lparen_t, expression_t, rparen_t>, \
factor_t <minus_t, lparen_t, expression_t, rparen_t>

//template <template <typename...> typename>
//struct term_t;

//template <>
//struct term_t <>//<factor_t>
//{
//    variant <
////    TOKENS
////    ph::number_t,
////    ph::plus_t,
////    ph::minus_t,
////    ph::multi_t,
////    ph::divi_t,
////    ph::lparen_t,
////    ph::rparen_t,
//    ph::factor_t <ph::number_t>,
//    ph::factor_t <ph::minus_t, ph::number_t>,
//    ph::factor_t <ph::lparen_t, ph::expression_t, ph::rparen_t>,
//    ph::factor_t <ph::minus_t, ph::lparen_t, ph::expression_t, ph::rparen_t>
////    ph::term_t,
////    ph::expression_t
//    > m_vari;
//
//
//
////    template <typename Variant>
////    term_t (Variant&& vari) : m_vari (forward <Variant> (vari))
////    {
////
////    }
//};

template <>
struct term_t <factor_t <number_t>>
{
    factor_t <number_t> m_vari;

//    term_t (auto&& v) : m_vari (forward <decltype (v)> (v))
//    {
//        cout << "tjo" << endl;
//    }

//    term_t () = default;
};

template <>
struct term_t <factor_t <minus_t, number_t>>
{
    factor_t <minus_t, number_t> m_vari;

    term_t (auto&& v) : m_vari (forward <decltype (v)> (v))
    {
        cout << "tjo" << endl;
    }

//    term_t () = default;
};

template <>
struct term_t <factor_t <lparen_t, expression_t, rparen_t>>
{
    using Factor = factor_t <lparen_t, expression_t, rparen_t>;
    Factor m_vari;

    term_t () = default;
//    term_t ()
};

//template <typename... T>

/**
  1  <expression> ::= <term>
  2                |  <expression> '+' <term>
  3                |  <expression> '-' <term>
  4  <term> ::= <factor>
  5          |  <term> '*' <factor>
  6          |  <term> '/' <factor>
  7  <factor> ::= <number>
  8            |  '-' <number>
  9            |  '(' <expression> ')'
 10            |  '-' '(' <expression> ')'

  1  <expression> ::= <term>
  2                |  <expression>  <plus>  <term>
  3                |  <expression>  <minus>  <term>

  4  <term>       ::= <factor>
  5                |  <term>  <multi>  <factor>
  6                |  <term>  <divi>  <factor>

  7  <factor>     ::= <number>
  8                |  <minus>  <number>
  9                |  <lparen>  <expression>  <rparen>
 10                |  <minus>  <lparen>  <expression>  <rparen>

 -4*5+6-(7+8)/97

<minus> <number> <multi> <number> <plus> <number> <minus> <lparen> <number> <plus> <number> <rparen> <divi> <number>

 */

using Token = variant <TOKENS>;

#define X(...) \
    template <typename... T> \
    constexpr auto operator== (variant <T...> const& rhs, ph::__VA_ARGS__ const& lhs) -> bool\
    { \
    return visit (overload {[](ph::__VA_ARGS__ const& lhs){return true;}, [](auto const&){return false;}}, rhs); \
    }

    MY_LIST
#undef X

#define X(...) \
    ostream& operator<< (ostream& os, ph::__VA_ARGS__ const& n) \
    { \
        return os <<  #__VA_ARGS__; \
    }

    MY_LIST
#undef X

#define get_token(x) \
[](auto&& tokens) -> decltype (auto)\
{ \
    return visit ([](auto&& t) -> decltype (auto) {return forward <decltype (t)> (t);}, forward <decltype (tokens)> (tokens)); \
}(x)

//#define X(x, ext) \
//    ph::x##ext const x {};
//
//    MY_LIST
//#undef X

//constexpr number_t number {};
//constexpr factor_t factor {};
//constexpr term_t term {};
//constexpr expression_t expression {};

}

//using Token = variant <ph::number,
//                       ph::plus_t,
//                       ph::minus_t,
//                       ph::multi_t,
//                       ph::divi_t,
//                       ph::lparen_t,
//                       ph::rparen_t>;

//#define PRIMITIVE_TOKENS ph::number_t, \
//ph::plus_t, \
//ph::minus_t, \
//ph::multi_t, \
//ph::divi_t, \
//ph::lparen_t, \
//ph::rparen_t
//
//#define ADVANCED_TOKENS ph::factor_t, \
//ph::term_t, \
//ph::expression_t
//
//#define TOKENS PRIMITIVE_TOKENS, \
//ADVANCED_TOKENS

template <typename T, typename... U>
ostream& operator<< (ostream& os, variant <T, U...> const& v)
{
    visit ([&os]<typename V>(V const& t){os<<t;}, v);
    return os;
}


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

  @copyright 2016 NumScale SAS
  @copyright 2016 J.T. Lapreste

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

#if defined(DOXYGEN_ONLY)
namespace boost { namespace simd
{

 /*!

    @ingroup group-trigonometric
    Function object implementing asind capabilities

    inverse sine in degree.

    @par Semantic:

    For every parameter of floating type T

    @code
    T r = asind(x);
    @endcode

    Returns the arc @c r in the interval
    \f$[-90, 90[\f$ such that <tt>sin(r) == x</tt>.
    If @c x is outside \f$[-1, 1[\f$ the result is Nan.

  **/
  const boost::dispatch::functor<tag::asind_> asind = {};
} }
#endif

#include <boost/simd/function/scalar/asind.hpp>
#include <boost/simd/function/simd/asind.hpp>

#endif


#ifndef ALGORITHM_CALCULATE_SEARCH_AREA_HPP_78GJFFW2930W
#define ALGORITHM_CALCULATE_SEARCH_AREA_HPP_78GJFFW2930W

#include <log4cxx/logger.h>
#include <log4cxx/basicconfigurator.h>
#include <log4cxx/helpers/exception.h>
#include <boost/thread.hpp>
#include <iostream>
#include <fstream>
#include "image.hpp"
#include <math.h>
#include <opencv2/core/core.hpp>

//namespace std {using namespace __gnu_cxx;}
using namespace std;

namespace prhlt
{
class Algorithm_Calculate_Search_Area
{
public:
  Algorithm_Calculate_Search_Area(cv::Mat image_mat, vector<vector<cv::Point> > ex_regions, vector<vector<vector<cv::Point> > > ex_baselines);
  vector<vector<cv::Rect> > run();

private:
  cv::Mat input_mat;
  Image global_input_image;
  vector<vector<cv::Rect > > search_areas;
  vector<vector<cv::Point > > regions;
  vector<vector<vector<cv::Point > > > baselines;
  log4cxx::LoggerPtr logger;
  int max_allowed_distance;
  int current_line_distance;
  void calculate_all_search_areas();
  void calculate_search_area(int region_id, int line_id);
  float distance_between_point_and_segment(cv::Point ex_point, cv::Point start_segment, cv::Point end_segment);
  cv::Point closest_point_on_segment_to_point(cv::Point ex_point, cv::Point start_segment, cv::Point end_segment);
  vector<vector<cv::Point > > calculate_lines_between_baselines(int ex_region_id, int ex_current_line, int ex_other_line);
  void show_rectangle(cv::Rect ex_rect);
  void show_lines(vector<vector<cv::Point> > ex_lines);
  void show_points(vector<cv::Point2d> ex_points);
};
} // namespace prhlt

#endif


#include <dlib/rand.h>
#include <dlib/svm_threaded.h>
#include <iostream>
#include <fstream>
#include <vector>
using namespace std;
using namespace dlib;

typedef matrix<double,2,1> sample_type;

int main(void){
    ofstream datafile;
    datafile.open ("multi_data_test.txt");

    const long num = 50;

    sample_type m;

    dlib::rand rnd;

    // make some samples near the origin
    double radius = 0.5;
    for (long i = 0; i < num+10; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        // add this sample to our set of training samples
//        samples.push_back(m);
 //       labels.push_back(1);
        datafile << m(0) << " " << m(1) << " 1\n";
    }

    // make some samples in a circle around the origin but far away
    radius = 10.0;
    for (long i = 0; i < num+20; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        // add this sample to our set of training samples
    //    samples.push_back(m);
     //   labels.push_back(2);
        datafile << m(0) << " " << m(1) << " 2\n";
    }

    // make some samples in a circle around the point (25,25)
    radius = 4.0;
    for (long i = 0; i < num+30; ++i)
    {
        double sign = 1;
        if (rnd.get_random_double() < 0.5)
            sign = -1;
        m(0) = 2*radius*rnd.get_random_double()-radius;
        m(1) = sign*sqrt(radius*radius - m(0)*m(0));

        // translate this point away from the origin
        m(0) += 25;
        m(1) += 25;

        datafile << m(0) << " " << m(1) << " 3\n";
    }
    datafile.close();
}


#include <boost/timer/timer.hpp>
#include <chrono>
#include "pandas.h"

int main() {
    namespace pd = pandas;
    using namespace std::chrono;
    using namespace std::literals::chrono_literals;

    constexpr auto market_open  = 1'518'186'600'000'000L; // 2018-02-09 09:30
    constexpr auto market_close = 1'518'210'000'000'000L; // 2018-02-09 16:00
    constexpr auto delta = duration_cast<microseconds>( 5s ).count();

    auto df = pd::read_csv<long, int, double, int, double, int>("quote.csv");
    df.set_index("recv_time");
    df = df[{"security_id", "bid_price", "ask_price"}];
    df = df.loc(market_open, market_close);

    std::vector<int> top_security_ids{3873, 3692, 1727, 3841, 1750};
    std::vector<pd::series> data;
    pd::series index;
    std::vector<std::string> columns;
    for (
        boost::timer::auto_cpu_timer t{3, "resampling mid price took %w seconds\n"};
        const auto &security_id : top_security_ids
    ) {
        auto df_sec = df.filter("security_id", security_id);
        df_sec = df_sec.resample(market_open, market_close, delta);
        data.push_back((df_sec["bid_price"] + df_sec["ask_price"]) / 2);
        index = df_sec.get_index();
        columns.push_back(std::to_string(security_id));
    }
    auto df_mid = pd::DataFrame(std::move(data), std::move(columns), std::move(index));
    std::cout << "mid price =" << std::endl << df_mid << std::endl;
    {
        boost::timer::auto_cpu_timer t{3, "calculating corr took %w seconds\n"};
        auto df_corr = df_mid.pct_change().corr();
        df_corr.set_index(pd::make_series(std::move(top_security_ids)));
        std::cout << "corr =" << std::endl << df_corr << std::endl;
    }

    return 0;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
//////////////////////////////////////////////////////////////////////////////
// cover test behavior of arithmetic components in scalar mode
//////////////////////////////////////////////////////////////////////////////

#include <nt2/arithmetic/include/functions/fma.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <vector>
#include <nt2/include/constants/valmin.hpp>
#include <nt2/include/constants/valmax.hpp>
#include <nt2/include/constants/sqrtvalmax.hpp>
#include <nt2/include/constants/mone.hpp>
#include <nt2/include/constants/one.hpp>

#include <nt2/sdk/unit/tests/cover.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/dispatch/meta/as_integer.hpp>

NT2_TEST_CASE_TPL ( fma_real,  NT2_REAL_TYPES)
{

  using nt2::fma;
  using nt2::tag::fma_;
  typedef typename nt2::meta::call<fma_(T, T, T)>::type r_t;

  nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR), in3(NR);
  std::vector<r_t> ref(NR);
  nt2::roll(in1, nt2::Mone<T>()/2, nt2::One<T>()/2);
  nt2::roll(in2, nt2::Mone<T>()/2, nt2::One<T>()/2);
  nt2::roll(in3, nt2::Mone<T>()/2, nt2::One<T>()/2);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = in1[i]*in2[i]+in3[i];
  }

 NT2_COVER_ULP_EQUAL(fma_, ((T, in1))((T, in2))((T, in3)), ref, 0);

}

NT2_TEST_CASE_TPL ( fma_ui, NT2_INTEGRAL_UNSIGNED_TYPES)
{

  using nt2::fma;
  using nt2::tag::fma_;
  typedef typename nt2::meta::call<fma_(T, T, T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type utype;

  static const nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR), in3(NR);
  std::vector<r_t> ref(NR);
  nt2::roll(in1, nt2::Valmin<T>(), nt2::Valmax<T>());
  nt2::roll(in2, nt2::Valmin<T>(), nt2::Valmax<T>());
  nt2::roll(in3, nt2::Valmin<T>(), nt2::Valmax<T>());
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = T(nt2::multiplies(utype(in1[i]), utype(in2[i]))+utype(in3[i]));
  }
 NT2_COVER_ULP_EQUAL(fma_, ((T, in1))((T, in2))((T, in3)), ref, 0);

}

NT2_TEST_CASE_TPL ( fma_si, NT2_INTEGRAL_SIGNED_TYPES)
{

  using nt2::fma;
  using nt2::tag::fma_;
  typedef typename nt2::meta::call<fma_(T, T, T)>::type r_t;
  typedef typename boost::dispatch::meta::as_integer<T, unsigned>::type utype;

  static const nt2::uint32_t NR = NT2_NB_RANDOM_TEST;
  std::vector<T> in1(NR), in2(NR), in3(NR);
  std::vector<r_t> ref(NR);
  nt2::roll(in1, -nt2::Sqrtvalmax<T>()/2, nt2::Sqrtvalmax<T>()/2);
  nt2::roll(in2, -nt2::Sqrtvalmax<T>()/2, nt2::Sqrtvalmax<T>()/2);
  nt2::roll(in3, -nt2::Sqrtvalmax<T>()/2, nt2::Sqrtvalmax<T>()/2);
  for(nt2::uint32_t i=0; i < NR ; ++i)
  {
    ref[i] = in1[i]*in2[i]+in3[i];
  }
 NT2_COVER_ULP_EQUAL(fma_, ((T, in1))((T, in2))((T, in3)), ref, 0);

}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//[ lazy_vector
// Defining this allows the assignment below of an expression to a double
// without writing any specific code to do so.
#include <boost/yap/expression.hpp>

#include <algorithm>
#include <cassert>
#include <iostream>
#include <vector>

template<boost::yap::expr_kind Kind, typename Tuple>
struct lazy_vector_expr;

// This transform turns a terminal of std::vector<double> into a terminal
// containing the nth double in that vector.  Think of it as turning our
// expression of vectors into an expression of scalars.
struct take_nth
{
    boost::yap::terminal<lazy_vector_expr, double> operator()(
        boost::yap::terminal<lazy_vector_expr, std::vector<double>> const &
            expr);

    std::size_t n;
};

// A custom expression template that defines lazy + and - operators that
// produce expressions, and an eager [] operator that returns the nth element
// of the expression.
//[ lazy_vector_decl
template<boost::yap::expr_kind Kind, typename Tuple>
struct lazy_vector_expr
{
    static const boost::yap::expr_kind kind = Kind;

    Tuple elements;

    // Note that this does not return an expression; it is greedily evaluated.
    auto operator[](std::size_t n) const;
};

BOOST_YAP_USER_BINARY_OPERATOR(plus, lazy_vector_expr, lazy_vector_expr)
BOOST_YAP_USER_BINARY_OPERATOR(minus, lazy_vector_expr, lazy_vector_expr)
//]

template<boost::yap::expr_kind Kind, typename Tuple>
auto lazy_vector_expr<Kind, Tuple>::operator[](std::size_t n) const
{
    return boost::yap::evaluate(boost::yap::transform(*this, take_nth{n}));
}

boost::yap::terminal<lazy_vector_expr, double> take_nth::operator()(
    boost::yap::terminal<lazy_vector_expr, std::vector<double>> const & expr)
{
    double x = boost::yap::value(expr)[n];
    // This move is something of a hack.  The move indicates that the terminal
    // should keep the value of x (since, being an rvalue, it may be a
    // temporary), rather than a reference to x.  See the "How Expression
    // Operands Are Treated" section of the tutorial for details.
    return boost::yap::make_terminal<lazy_vector_expr, double>(std::move(x));
}

// In order to define the += operator with the semantics we want, it's
// convenient to derive a terminal type from a terminal instantiation of
// lazy_vector_expr.  note that we could have written a template
// specialization here instead -- either one would work.  That would of course
// have required more typing.
struct lazy_vector : lazy_vector_expr<
                         boost::yap::expr_kind::terminal,
                         boost::hana::tuple<std::vector<double>>>
{
    lazy_vector() {}

    explicit lazy_vector(std::vector<double> && vec)
    {
        elements = boost::hana::tuple<std::vector<double>>(std::move(vec));
    }

    template<boost::yap::expr_kind Kind, typename Tuple>
    lazy_vector & operator+=(lazy_vector_expr<Kind, Tuple> const & rhs)
    {
        std::vector<double> & this_vec = boost::yap::value(*this);
        for (int i = 0, size = (int)this_vec.size(); i < size; ++i) {
            this_vec[i] += rhs[i];
        }
        return *this;
    }
};

lazy_vector v1{std::vector<double>(4, 1.0)};
lazy_vector v2{std::vector<double>(4, 2.0)};
lazy_vector v3{std::vector<double>(4, 3.0)};

double get_d1_with_yap()
{
    double retval = (v2 + v3)[2];
    return retval;
}

double get_d1_by_hand()
{
    std::vector<double> & v2_ref = boost::yap::value(v2);
    std::vector<double> & v3_ref = boost::yap::value(v3);
    double retval = v2_ref[2] + v3_ref[2];
    return retval;
}

void update_v1_with_yap() { v1 += v2 - v3; }

void update_v1_by_hand()
{
    std::vector<double> & v1_ref = boost::yap::value(v1);
    std::vector<double> & v2_ref = boost::yap::value(v2);
    std::vector<double> & v3_ref = boost::yap::value(v3);
    for (int i = 0, size = (int)v1_ref.size(); i < size; ++i) {
        v1_ref[i] += v2_ref[i] - v3_ref[i];
    }
}

int main()
{
    double d1_1 = get_d1_with_yap();
    std::cout << d1_1 << "\n";

    double d1_2 = get_d1_by_hand();
    std::cout << d1_2 << "\n";

    update_v1_with_yap();
    std::cout << '{' << v1[0] << ',' << v1[1] << ',' << v1[2] << ',' << v1[3]
              << '}' << "\n";

    boost::yap::value(v1) = std::vector<double>(4, 1.0);

    update_v1_by_hand();
    std::cout << '{' << v1[0] << ',' << v1[1] << ',' << v1[2] << ',' << v1[3]
              << '}' << "\n";

    // This expression is disallowed because it does not conform to the
    // implicit grammar.  operator+= is only defined on terminals, not
    // arbitrary expressions.
    // (v2 + v3) += v1;

    return 0;
}
//]


/*
Authors: Deevashwer Rathee
Copyright:
Copyright (c) 2021 Microsoft Research
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.
*/

#include "LinearOT/linear-ot.h"
#include <cmath>

#define MAX_NUM_OT (1 << 20)

#define USE_EIGEN
#ifdef USE_EIGEN
#include <Eigen/Dense>
#endif

using namespace std;
using namespace sci;

void matrix_transpose(uint64_t *A, int32_t m, int32_t n, int d = 1) {
  uint64_t *tmpA = new uint64_t[m * n * d];
  memcpy(tmpA, A, m * n * d * sizeof(uint64_t));
  for (int i = 0; i < m; i++) {
    for (int j = 0; j < n; j++) {
      if (d == 1) {
        A[j * m + i] = tmpA[i * n + j];
      } else {
        memcpy(A + (j * m + i) * d, tmpA + (i * n + j) * d,
               d * sizeof(uint64_t));
      }
    }
  }
  delete[] tmpA;
}

LinearOT::LinearOT(int party, NetIO *io, OTPack<NetIO> *otpack) {
  this->party = party;
  this->io = io;
  this->otpack = otpack;
  this->aux = new AuxProtocols(party, io, otpack);
  this->trunc = new Truncation(party, io, otpack, aux);
  this->xt = new XTProtocol(party, io, otpack, aux);
}

LinearOT::~LinearOT() {
  delete aux;
  delete trunc;
}

void LinearOT::hadamard_cleartext(int dim, uint64_t *inA, uint64_t *inB,
                                  uint64_t *outC) {
  matmul_cleartext(1, dim, 1, inA, inB, outC, false);
}

#ifdef USE_EIGEN
void matmul_cleartext_eigen(int dim1, int dim2, int dim3, uint64_t *inA,
                            uint64_t *inB, uint64_t *outC) {
  Eigen::Matrix<uint64_t, Eigen::Dynamic, Eigen::Dynamic> eigen_A(dim1, dim2);
  Eigen::Matrix<uint64_t, Eigen::Dynamic, Eigen::Dynamic> eigen_B(dim2, dim3);
  Eigen::Matrix<uint64_t, Eigen::Dynamic, Eigen::Dynamic> eigen_C(dim1, dim3);

  for (int i = 0; i < dim1; i++) {
    for (int j = 0; j < dim2; j++) {
      eigen_A(i, j) = Arr2DIdxRowM(inA, dim1, dim2, i, j);
    }
  }
  for (int i = 0; i < dim2; i++) {
    for (int j = 0; j < dim3; j++) {
      eigen_B(i, j) = Arr2DIdxRowM(inB, dim2, dim3, i, j);
    }
  }
  eigen_C = eigen_A * eigen_B;
  for (int i = 0; i < dim1; i++) {
    for (int j = 0; j < dim3; j++) {
      Arr2DIdxRowM(outC, dim1, dim3, i, j) = eigen_C(i, j);
    }
  }
}
#endif

void LinearOT::matmul_cleartext(int dim1, int dim2, int dim3, uint64_t *inA,
                                uint64_t *inB, uint64_t *outC,
                                bool accumulate) {
  if (!accumulate) {
    for (int i = 0; i < dim1; i++) {
      for (int j = 0; j < dim3; j++) {
        for (int k = 0; k < dim2; k++) {
          outC[dim2 * dim3 * i + dim2 * j + k] =
              inA[dim2 * i + k] * inB[dim3 * k + j];
        }
      }
    }
    return;
  }
#ifndef USE_EIGEN
  for (int i = 0; i < dim1; i++) {
    for (int j = 0; j < dim3; j++) {
      outC[i * dim3 + j] = 0;
      for (int k = 0; k < dim2; k++) {
        outC[i * dim3 + j] += (inA[dim2 * i + k] * inB[dim3 * k + j]);
      }
    }
  }
#else
  assert(accumulate == true && "Eigen not configured for accumulate = false");
  matmul_cleartext_eigen(dim1, dim2, dim3, inA, inB, outC);
#endif
}

void LinearOT::hadamard_cross_terms(int32_t dim, uint64_t *inA, uint64_t *inB,
                                    uint64_t *outC, int32_t bwA, int32_t bwB,
                                    int32_t bwC, MultMode mode) {
  matmul_cross_terms(1, dim, 1, inA, inB, outC, bwA, bwB, bwC, false, mode);
}

void LinearOT::hadamard_product(int32_t dim, uint64_t *inA, uint64_t *inB,
                                uint64_t *outC, int32_t bwA, int32_t bwB,
                                int32_t bwC, bool signed_arithmetic,
                                bool signed_B, MultMode mode, uint8_t *msbA,
                                uint8_t *msbB) {
  matrix_multiplication(1, dim, 1, inA, inB, outC, bwA, bwB, bwC,
                        signed_arithmetic, signed_B, false, mode, msbA, msbB);
}

void LinearOT::matmul_cross_terms(int32_t dim1, int32_t dim2, int32_t dim3,
                                  uint64_t *inA, uint64_t *inB, uint64_t *outC,
                                  int32_t bwA, int32_t bwB, int32_t bwC,
                                  bool accumulate, MultMode mode) {
  bool use_straight_ot = false, use_reversed_ot = false;
  uint64_t maskC = (bwC == 64 ? -1 : ((1ULL << bwC) - 1));
  uint64_t *inS, *inR;
  int32_t bwR, dimS1, dimS2, dimR1, dimR2;
  // A whole row of values is multiplied to a element using only bwR OTs
  bool row_batching;
  if (mode == MultMode::Alice_has_A) {
    use_straight_ot = true;
    row_batching = false;
  } else if (mode == MultMode::Bob_has_A) {
    use_straight_ot = true;
    row_batching = true;
  } else if (mode == MultMode::Alice_has_B) {
    use_reversed_ot = true;
    row_batching = false;
  } else if (mode == MultMode::Bob_has_B) {
    use_reversed_ot = true;
    row_batching = true;
  } else {
    use_straight_ot = true;
    use_reversed_ot = true;
    // if (bwA*dim1*dim2 > bwB*dim2*dim3) {
    if (bwA > bwB) {
      row_batching = false;
    } else {
      row_batching = true;
    }
  }
  if (row_batching) {
    inS = inB;
    inR = inA;
    bwR = bwA;
    dimS1 = dim2;
    dimS2 = dim3;
    dimR1 = dim1;
    dimR2 = dim2;
  } else {
    // inS = inA;
    inS = new uint64_t[dim1 * dim2];
    memcpy(inS, inA, dim1 * dim2 * sizeof(uint64_t));
    inR = inB;
    bwR = bwB;
    dimS1 = dim1;
    dimS2 = dim2;
    dimR1 = dim2;
    dimR2 = dim3;
  }

  int32_t num_ot = dimR1 * dimR2 * bwR;
  int32_t msgs_per_ot = (row_batching ? dimS2 : dimS1);
  int32_t dim = dimR1 * dimR2;
  // max_num_ot is multiple of dim
  int32_t max_num_ot = ceil(float(MAX_NUM_OT) / (dim * msgs_per_ot)) * dim;
  int32_t batch_size;
  if (num_ot < max_num_ot)
    batch_size = num_ot;
  else
    batch_size = max_num_ot;

  uint64_t *corr = new uint64_t[batch_size * msgs_per_ot];
  uint64_t *ABs = new uint64_t[batch_size * msgs_per_ot];
  uint64_t *ABr = new uint64_t[batch_size * msgs_per_ot];
  bool *choice = new bool[batch_size];
  uint64_t *tmpR = new uint64_t[dim];
  memcpy(tmpR, inR, dim * sizeof(uint64_t));

  memset(ABs, 0, batch_size * msgs_per_ot * sizeof(uint64_t));
  memset(ABr, 0, batch_size * msgs_per_ot * sizeof(uint64_t));
  if (accumulate) {
    memset(outC, 0, dim1 * dim3 * sizeof(uint64_t));
  } else {
    memset(outC, 0, dim1 * dim2 * dim3 * sizeof(uint64_t));
  }
  if (!row_batching) {
    // inplace transposing is fine because inS is a copy if row_batching = false
    matrix_transpose(inS, dimS1, dimS2);
  }

  for (int i = 0; i < num_ot; i += batch_size) {
    vector<int> msg_len;
    if (batch_size <= num_ot - i)
      msg_len.resize(batch_size / dim);
    else
      msg_len.resize((num_ot - i) / dim);
    for (int j = i; j < i + batch_size and j < num_ot; j += dim) {
      int bit_offset = i / dim;
      int bit_idx = j / dim;
      msg_len[bit_idx - bit_offset] = bwC - bit_idx;
      for (int k = j; k < j + dim and k < i + batch_size; k++) {
        int inp_idx = k - j;
        int row_idx_R = inp_idx / dimR2;
        int col_idx_R = inp_idx % dimR2;
        for (int h = 0; h < msgs_per_ot; h++) {
          uint64_t elemS;
          if (row_batching)
            elemS = inS[col_idx_R * dimS2 + h];
          // To preserve locality, inS is tranposed if row_batching = false
          else
            elemS = inS[row_idx_R * dimS1 + h];
          // else elemS = inS[h*dimS2 + row_idx_R];
          corr[(k - i) * msgs_per_ot + h] =
              ((elemS << bit_idx) & maskC) >> bit_idx;
        }
        choice[k - i] = (bool)(tmpR[inp_idx] & 1);
        tmpR[inp_idx] >>= 1;
      }
    }
    if (use_straight_ot) {
      if (party == sci::ALICE) {
        if (batch_size <= num_ot - i) {
          otpack->iknp_straight->send_batched_cot(ABs, corr, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_straight->send_batched_cot(ABs, corr, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      } else { // party == sci::BOB
        if (batch_size <= num_ot - i) {
          otpack->iknp_straight->recv_batched_cot(ABr, choice, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_straight->recv_batched_cot(ABr, choice, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      }
    }
    if (use_reversed_ot) {
      if (party == sci::ALICE) {
        if (batch_size <= num_ot - i) {
          otpack->iknp_reversed->recv_batched_cot(ABr, choice, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_reversed->recv_batched_cot(ABr, choice, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      } else { // party == sci::BOB
        if (batch_size <= num_ot - i) {
          otpack->iknp_reversed->send_batched_cot(ABs, corr, msg_len,
                                                  batch_size, msgs_per_ot);
        } else {
          otpack->iknp_reversed->send_batched_cot(ABs, corr, msg_len,
                                                  num_ot - i, msgs_per_ot);
        }
      }
    }
    for (int h = 0; h < dim2; h++) {
      for (int j = i; j < i + batch_size and j < num_ot; j += dim) {
        int bit_idx = j / dim;
        for (int k = 0; k < dim1 * dim3; k++) {
          int row_idx = (row_batching ? k / dim3 : k / dim1);
          int col_idx = (row_batching ? k % dim3 : k % dim1);
          int idx;
          if (row_batching)
            idx = (j - i + row_idx * dimR2 + h) * msgs_per_ot + col_idx;
          // To preserve locality, outC-transpose is computed if row_batching =
          // false
          else
            idx = (j - i + h * dimR2 + row_idx) * msgs_per_ot + col_idx;
          if (use_straight_ot) {
            uint64_t temp = (party == ALICE ? -ABs[idx] : ABr[idx]) << bit_idx;
            if (accumulate) {
              outC[k] += temp;
            } else {
              outC[k * dim2 + h] += temp;
            }
          }
          if (use_reversed_ot) {
            uint64_t temp = (party == BOB ? -ABs[idx] : ABr[idx]) << bit_idx;
            if (accumulate) {
              outC[k] += temp;
            } else {
              outC[k * dim2 + h] += temp;
            }
          }
        }
      }
    }
  }
  if (!row_batching) {
    if (accumulate) {
      matrix_transpose(outC, dim3, dim1);
    } else {
      matrix_transpose(outC, dim3, dim1, dim2);
    }
  }

  if (accumulate) {
    for (int k = 0; k < dim1 * dim3; k++) {
      outC[k] &= maskC;
    }
  } else {
    for (int k = 0; k < dim1 * dim2 * dim3; k++) {
      outC[k] &= maskC;
    }
  }

  delete[] corr;
  delete[] ABs;
  delete[] ABr;
  delete[] choice;
  delete[] tmpR;
  if (!row_batching)
    delete[] inS;
}

void LinearOT::matmul_multiplexer(int32_t dim1, int32_t dim2, int32_t dim3,
                                  uint64_t *inA, uint64_t *inB, uint64_t *outC,
                                  int32_t bwA, int32_t bwB, int32_t bwC,
                                  bool accumulate, MultMode mode) {
  assert(bwA == 1 || bwB == 1);
  assert(bwC < (bwA + bwB));

  bool use_straight_ot = false, use_reversed_ot = false;
  uint64_t maskC = (bwC == 64 ? -1 : ((1ULL << bwC) - 1));
  uint64_t *inS, *inR;
  int32_t dimS1, dimS2, dimR1, dimR2;
  // A whole row of values is multiplied to a bit
  bool row_batching;
  if (bwB == 1) {
    inS = new uint64_t[dim1 * dim2];
    memcpy(inS, inA, dim1 * dim2 * sizeof(uint64_t));
    inR = inB;
    dimS1 = dim1;
    dimS2 = dim2;
    dimR1 = dim2;
    dimR2 = dim3;
    if (mode == MultMode::Alice_has_A)
      use_straight_ot = true;
    else if (mode == MultMode::Bob_has_A)
      use_reversed_ot = true;
    else {
      use_straight_ot = true;
      use_reversed_ot = true;
    }
    row_batching = false;
  } else { // bwA == 1
    inS = inB;
    inR = inA;
    dimS1 = dim2;
    dimS2 = dim3;
    dimR1 = dim1;
    dimR2 = dim2;
    if (mode == MultMode::Alice_has_B)
      use_straight_ot = true;
    else if (mode == MultMode::Bob_has_B)
      use_reversed_ot = true;
    else {
      use_straight_ot = true;
      use_reversed_ot = true;
    }
    row_batching = true;
  }

  int32_t dim = dimR1 * dimR2;
  int32_t msgs_per_ot = (row_batching ? dimS2 : dimS1);

  PRG128 prg;
  uint64_t *data = new uint64_t[2 * dim * msgs_per_ot];
  uint64_t *ABs = new uint64_t[dim * msgs_per_ot];
  uint64_t *ABr = new uint64_t[dim * msgs_per_ot];
  uint8_t *choice = new uint8_t[dim];

  if (accumulate) {
    for (int k = 0; k < dim1 * dim3; k++) {
      outC[k] = 0;
    }
  } else {
    for (int k = 0; k < dim1 * dim2 * dim3; k++) {
      outC[k] = 0;
    }
  }
  memset(ABs, 0, dim * msgs_per_ot * sizeof(uint64_t));
  memset(ABr, 0, dim * msgs_per_ot * sizeof(uint64_t));

  if (!row_batching) {
    // inplace transposing is fine because inS is a copy if row_batching = false
    matrix_transpose(inS, dimS1, dimS2);
  }

  if (use_straight_ot && party == ALICE) {
    prg.random_data(ABs, dim * msgs_per_ot * sizeof(uint64_t));
  }
  if (use_reversed_ot && party == BOB) {
    prg.random_data(ABs, dim * msgs_per_ot * sizeof(uint64_t));
  }

  for (int i = 0; i < dim; i++) {
    int row_idx_R = i / dimR2;
    int col_idx_R = i % dimR2;

    choice[i] = (bool)(inR[i] & 1);
    uint64_t choice_bit = choice[i];
    for (int h = 0; h < msgs_per_ot; h++) {
      uint64_t elemS;
      if (row_batching)
        elemS = inS[col_idx_R * dimS2 + h];
      // To preserve locality, inS is tranposed if row_batching = false
      else
        elemS = inS[row_idx_R * dimS1 + h];
      int idx = i * msgs_per_ot + h;
      data[idx * 2] = (ABs[idx] + elemS * choice_bit) & maskC;
      data[idx * 2 + 1] = (ABs[idx] + elemS * (1 - choice_bit)) & maskC;
    }
  }
  if (party == sci::ALICE) {
    if (use_straight_ot) {
      otpack->iknp_straight->send_batched_got(data, dim, bwC, msgs_per_ot);
    }
    if (use_reversed_ot) {
      otpack->iknp_reversed->recv_batched_got(ABr, choice, dim, bwC,
                                              msgs_per_ot);
    }
  } else { // party == sci::BOB
    if (use_straight_ot) {
      otpack->iknp_straight->recv_batched_got(ABr, choice, dim, bwC,
                                              msgs_per_ot);
    }
    if (use_reversed_ot) {
      otpack->iknp_reversed->send_batched_got(data, dim, bwC, msgs_per_ot);
    }
  }
  for (int h = 0; h < dim2; h++) {
    for (int k = 0; k < dim1 * dim3; k++) {
      int row_idx = (row_batching ? k / dim3 : k / dim1);
      int col_idx = (row_batching ? k % dim3 : k % dim1);
      int idx;
      if (row_batching)
        idx = (row_idx * dimR2 + h) * msgs_per_ot + col_idx;
      // To preserve locality, outC-transpose is computed if row_batching =
      // false
      else
        idx = (h * dimR2 + row_idx) * msgs_per_ot + col_idx;
      if (use_straight_ot) {
        uint64_t temp = (party == ALICE ? -ABs[idx] : ABr[idx]);
        if (accumulate) {
          outC[k] += temp;
        } else {
          outC[k * dim2 + h] += temp;
        }
      }
      if (use_reversed_ot) {
        uint64_t temp = (party == BOB ? -ABs[idx] : ABr[idx]);
        if (accumulate) {
          outC[k] += temp;
        } else {
          outC[k * dim2 + h] += temp;
        }
      }
    }
  }
  if (!row_batching) {
    if (accumulate) {
      matrix_transpose(outC, dim3, dim1);
    } else {
      matrix_transpose(outC, dim3, dim1, dim2);
    }
  }

  if (accumulate) {
    for (int k = 0; k < dim1 * dim3; k++) {
      outC[k] &= maskC;
    }
  } else {
    for (int k = 0; k < dim1 * dim2 * dim3; k++) {
      outC[k] &= maskC;
    }
  }

  delete[] data;
  delete[] ABs;
  delete[] ABr;
  delete[] choice;
  if (!row_batching)
    delete[] inS;
}

void LinearOT::matrix_multiplication(int32_t dim1, int32_t dim2, int32_t dim3,
                                     uint64_t *inA, uint64_t *inB,
                                     uint64_t *outC, int32_t bwA, int32_t bwB,
                                     int32_t bwC, bool signed_arithmetic,
                                     bool signed_B, bool accumulate,
                                     MultMode mode, uint8_t *msbA,
                                     uint8_t *msbB) {
  assert(bwC <= 64);
  assert((bwC <= (bwA + bwB)) && (bwC >= bwA) && (bwC >= bwB));
  int32_t extra_bits = (accumulate ? ceil(log2(dim2)) : 0);
  uint64_t *tmpA = new uint64_t[dim1 * dim2];
  uint64_t *tmpB = new uint64_t[dim2 * dim3];

  bool sender_A = false;
  bool sender_B = false;
  if (mode == MultMode::Alice_has_A || mode == MultMode::Alice_has_B) {
    sender_A = true;
  } else if (mode == MultMode::Bob_has_A || mode == MultMode::Bob_has_B) {
    sender_B = true;
  } else {
    if (bwA > bwB) {
      sender_A = true;
    } else {
      sender_B = true;
    }
  }
  if (sender_A) {
    if (mode == MultMode::Alice_has_A || mode == MultMode::Bob_has_A) {
      for (int i = 0; i < dim1 * dim2; i++) {
        tmpA[i] = (signed_arithmetic ? signed_val(inA[i], bwA) : inA[i]);
      }
    } else {
      if (signed_arithmetic) {
        xt->s_extend(dim1 * dim2, inA, tmpA, bwA, bwA + extra_bits, msbA);
      } else {
        xt->z_extend(dim1 * dim2, inA, tmpA, bwA, bwA + extra_bits, msbA);
      }
    }
    memcpy(tmpB, inB, dim2 * dim3 * sizeof(uint64_t));
    bwA = bwA + extra_bits;
  } else if (sender_B) {
    if (mode == MultMode::Alice_has_B || mode == MultMode::Bob_has_B) {
      for (int i = 0; i < dim2 * dim3; i++) {
        tmpB[i] = ((signed_arithmetic && signed_B) ? signed_val(inB[i], bwB)
                                                   : inB[i]);
      }
    } else {
      if (signed_arithmetic && signed_B) {
        xt->s_extend(dim2 * dim3, inB, tmpB, bwB, bwB + extra_bits, msbB);
      } else {
        xt->z_extend(dim2 * dim3, inB, tmpB, bwB, bwB + extra_bits, msbB);
      }
    }
    memcpy(tmpA, inA, dim1 * dim2 * sizeof(uint64_t));
    bwB = bwB + extra_bits;
  }
  bwC = bwC + extra_bits;
  uint64_t maskA = (bwA == 64 ? -1 : ((1ULL << bwA) - 1));
  uint64_t maskB = (bwB == 64 ? -1 : ((1ULL << bwB) - 1));
  uint64_t maskC = (bwC == 64 ? -1 : ((1ULL << bwC) - 1));
  uint64_t pow_A = 1ULL << bwA;
  uint64_t pow_B = 1ULL << bwB;
  uint64_t pow_A_2 = (1ULL << (bwA - 1));
  uint64_t pow_B_2 = (1ULL << (bwB - 1));
  int32_t dim;
  if (accumulate)
    dim = dim1 * dim3;
  else
    dim = dim1 * dim2 * dim3;

  for (int i = 0; i < dim1 * dim2; i++) {
    if (signed_arithmetic) {
      if (mode == MultMode::Alice_has_A) {
        tmpA[i] = (party == ALICE ? tmpA[i] + pow_A_2 : 0) & maskA;
      } else if (mode == MultMode::Bob_has_A) {
        tmpA[i] = (party == BOB ? tmpA[i] + pow_A_2 : 0) & maskA;
      } else {
        tmpA[i] = (tmpA[i] + (party == BOB ? pow_A_2 : 0)) & maskA;
      }
    } else {
      tmpA[i] = tmpA[i] & maskA;
    }
  }
  for (int i = 0; i < dim2 * dim3; i++) {
    if (signed_arithmetic && signed_B) {
      if (mode == MultMode::Alice_has_B) {
        tmpB[i] = (party == ALICE ? tmpB[i] + pow_B_2 : 0) & maskB;
      } else if (mode == MultMode::Bob_has_B) {
        tmpB[i] = (party == BOB ? tmpB[i] + pow_B_2 : 0) & maskB;
      } else {
        tmpB[i] = (tmpB[i] + (party == BOB ? pow_B_2 : 0)) & maskB;
      }
    } else {
      tmpB[i] = tmpB[i] & maskB;
    }
  }
  uint64_t *cross_terms = new uint64_t[dim];
  matmul_cross_terms(dim1, dim2, dim3, tmpA, tmpB, cross_terms, bwA, bwB, bwC,
                     accumulate, mode);

  uint64_t *local_terms = new uint64_t[dim];
  if (party == ALICE &&
      (mode == MultMode::Alice_has_A || mode == MultMode::Alice_has_B)) {
    matmul_cleartext(dim1, dim2, dim3, tmpA, tmpB, local_terms, accumulate);
  } else if (party == BOB &&
             (mode == MultMode::Bob_has_A || mode == MultMode::Bob_has_B)) {
    matmul_cleartext(dim1, dim2, dim3, tmpA, tmpB, local_terms, accumulate);
  } else if (mode == MultMode::None) {
    matmul_cleartext(dim1, dim2, dim3, tmpA, tmpB, local_terms, accumulate);
  } else {
    memset(local_terms, 0, dim * sizeof(uint64_t));
  }

  uint8_t *wA = new uint8_t[dim1 * dim2];
  uint8_t *wB = new uint8_t[dim2 * dim3];
  uint64_t *wA_B = new uint64_t[dim];
  uint64_t *wB_A = new uint64_t[dim];

  if (bwC > bwA) {
    if (mode == MultMode::Alice_has_A || mode == MultMode::Bob_has_A) {
      memset(wA, 0, dim1 * dim2 * sizeof(uint8_t));
      memset(wA_B, 0, dim * sizeof(uint64_t));
    } else {
      if (msbA != nullptr) {
        uint8_t *tmp_msbA = new uint8_t[dim1 * dim2];
        if (signed_arithmetic) {
          for (int i = 0; i < dim1 * dim2; i++) {
            tmp_msbA[i] = (party == ALICE ? msbA[i] ^ 1 : msbA[i]);
          }
        } else {
          for (int i = 0; i < dim1 * dim2; i++) {
            tmp_msbA[i] = (sender_A && (extra_bits > 0) ? 0 : msbA[i]);
          }
        }
        aux->MSB_to_Wrap(tmpA, tmp_msbA, wA, dim1 * dim2, bwA);
        delete[] tmp_msbA;
      } else {
        aux->wrap_computation(tmpA, wA, dim1 * dim2, bwA);
      }
      uint64_t *wA64 = new uint64_t[dim1 * dim2];
      for (int i = 0; i < dim1 * dim2; i++) {
        wA64[i] = uint64_t(wA[i]);
      }
      matmul_multiplexer(dim1, dim2, dim3, wA64, tmpB, wA_B, 1, bwB, bwC - bwA,
                         accumulate, mode);
      delete[] wA64;
    }
  }
  if (bwC > bwB) {
    if (mode == MultMode::Alice_has_B || mode == MultMode::Bob_has_B) {
      memset(wB, 0, dim2 * dim3 * sizeof(uint8_t));
      memset(wB_A, 0, dim * sizeof(uint64_t));
    } else {
      if (msbB != nullptr) {
        uint8_t *tmp_msbB = new uint8_t[dim2 * dim3];
        if (signed_arithmetic && signed_B) {
          for (int i = 0; i < dim2 * dim3; i++) {
            tmp_msbB[i] = (party == ALICE ? msbB[i] ^ 1 : msbB[i]);
          }
        } else {
          for (int i = 0; i < dim2 * dim3; i++) {
            tmp_msbB[i] = (sender_B && (extra_bits > 0) ? 0 : msbB[i]);
          }
        }
        aux->MSB_to_Wrap(tmpB, tmp_msbB, wB, dim2 * dim3, bwB);
        delete[] tmp_msbB;
      } else {
        aux->wrap_computation(tmpB, wB, dim2 * dim3, bwB);
      }
      uint64_t *wB64 = new uint64_t[dim2 * dim3];
      for (int i = 0; i < dim2 * dim3; i++) {
        wB64[i] = uint64_t(wB[i]);
      }
      matmul_multiplexer(dim1, dim2, dim3, tmpA, wB64, wB_A, bwA, 1, bwC - bwB,
                         accumulate, mode);
      delete[] wB64;
    }
  }

  uint64_t *tmpC = new uint64_t[dim];
  int inner_loop_size = (accumulate ? dim2 : 1);
  for (int i = 0; i < dim; i++) {
    tmpC[i] =
        (local_terms[i] + cross_terms[i] - pow_A * wA_B[i] - pow_B * wB_A[i]) &
        maskC;
    if (signed_arithmetic) {
      for (int j = 0; j < inner_loop_size; j++) {
        int idx = (accumulate ? i : i / dim2);
        int common_idx = (accumulate ? j : i % dim2);
        int row_idx = idx / dim3;
        int col_idx = idx % dim3;
        int A_idx = row_idx * dim2 + common_idx;
        int B_idx = common_idx * dim3 + col_idx;
        if (signed_B) {
          if (party == ALICE) {
            tmpC[i] = (tmpC[i] - pow_A_2 * (tmpB[B_idx] - pow_B * wB[B_idx]) -
                       pow_B_2 * (tmpA[A_idx] - pow_A * wA[A_idx])) &
                      maskC;
          } else { // party == BOB
            tmpC[i] = (tmpC[i] - pow_A_2 * (tmpB[B_idx] - pow_B * wB[B_idx]) -
                       pow_B_2 * (tmpA[A_idx] - pow_A * wA[A_idx]) +
                       pow_A_2 * pow_B_2) &
                      maskC;
          }
        } else {
          tmpC[i] =
              (tmpC[i] - pow_A_2 * (tmpB[B_idx] - pow_B * wB[B_idx])) & maskC;
        }
      }
    }
  }
  if (accumulate) {
    trunc->truncate_and_reduce(dim1 * dim3, tmpC, outC, extra_bits, bwC);
  } else {
    memcpy(outC, tmpC, dim * sizeof(uint64_t));
  }

  delete[] cross_terms;
  delete[] local_terms;
  delete[] wA;
  delete[] wB;
  delete[] wA_B;
  delete[] wB_A;
  delete[] tmpA;
  delete[] tmpB;
  delete[] tmpC;
}


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2012   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#define NT2_UNIT_MODULE "nt2 boost.simd.arithmetic toolbox - iround2even/simd Mode"

//////////////////////////////////////////////////////////////////////////////
// unit test behavior of boost.simd.arithmetic components in simd mode
//////////////////////////////////////////////////////////////////////////////
/// created by jt the 01/12/2010
///
#include <boost/simd/arithmetic/include/functions/iround2even.hpp>
#include <boost/simd/sdk/simd/io.hpp>
#include <boost/dispatch/meta/as_integer.hpp>
#include <nt2/sdk/unit/tests.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <boost/simd/sdk/simd/native.hpp>
#include <boost/simd/constant/constant.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/one.hpp>
#include <boost/simd/include/constants/two.hpp>
#include <boost/simd/include/constants/inf.hpp>
#include <boost/simd/include/constants/zero.hpp>
#include <boost/simd/include/constants/minf.hpp>
#include <boost/simd/include/constants/mone.hpp>
#include <boost/simd/include/constants/nan.hpp>

NT2_TEST_CASE_TPL ( iround2even_real__1_0,  BOOST_SIMD_SIMD_REAL_TYPES)
{
  using boost::simd::iround2even;
  using boost::simd::tag::iround2even_;
  using boost::simd::native;
  typedef BOOST_SIMD_DEFAULT_EXTENSION  ext_t;
  typedef native<T,ext_t>                  vT;
  typedef typename boost::dispatch::meta::call<iround2even_(vT)>::type r_t;

  // specific values tests
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::splat<vT>(1.4)), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::splat<vT>(1.5)), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::splat<vT>(1.6)), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::splat<vT>(2.5)), boost::simd::Two<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Half<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Inf<vT>()), boost::simd::Inf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Mhalf<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Minf<vT>()), boost::simd::Minf<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Mone<vT>()), boost::simd::Mone<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Nan<vT>()), boost::simd::Zero<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::One<vT>()), boost::simd::One<r_t>(), 0);
  NT2_TEST_ULP_EQUAL(iround2even(boost::simd::Zero<vT>()), boost::simd::Zero<r_t>(), 0);

  for(int i=-100; i < 100; ++i)
 {
   NT2_TEST_ULP_EQUAL(iround2even(boost::simd::splat<vT>(i+0.5)),boost::simd::splat<r_t>(i+((i >= 0)?(i%2):(-i%2))), 0);
 }
} // end of test for floating_


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

  @copyright 2016 NumScale SAS

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

#include <boost/simd/constant/half.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_finite.hpp>
#include <boost/simd/function/logical_and.hpp>
#include <boost/simd/function/max.hpp>
#include <boost/simd/function/min.hpp>
#include <boost/simd/function/minus.hpp>
#include <boost/simd/function/plus.hpp>
#include <boost/simd/function/shift_right.hpp>
#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
//no overflow average for floating numbers

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  BOOST_DISPATCH_OVERLOAD_IF ( meanof_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::floating_<A0>, X>
                          , bs::pack_< bd::floating_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      A0 m = min(a0, a1);
      return if_else( logical_and(is_finite(a0), is_finite(a1)),
                      m + (max(a0, a1)-m)*Half<A0>(),
                      a0+a1
                    );
    }
  };
  BOOST_DISPATCH_OVERLOAD_IF ( meanof_
                          , (typename A0, typename X)
                          , (detail::is_native<X>)
                          , bd::cpu_
                          , bs::pack_< bd::integer_<A0>, X>
                          , bs::pack_< bd::integer_<A0>, X>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( A0 const& a0, A0 const& a1) const BOOST_NOEXCEPT
    {
      return (a0 & a1) + ((a0 ^ a1) >> 1);
    }
  };
} } }

#endif


// Copyright (C) 2013 The Regents of the University of California (Regents).
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//
//     * Redistributions in binary form must reproduce the above
//       copyright notice, this list of conditions and the following
//       disclaimer in the documentation and/or other materials provided
//       with the distribution.
//
//     * Neither the name of The Regents or University of California nor the
//       names of its contributors may be used to endorse or promote products
//       derived from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Please contact the author of this library if you have any questions.
// Author: Chris Sweeney (cmsweeney@cs.ucsb.edu)

#include "theia/sfm/pose/essential_matrix_utils.h"

#include <Eigen/Core>
#include <Eigen/Geometry>
#include <glog/logging.h>

#include <algorithm>

#include "theia/matching/feature_correspondence.h"
#include "theia/sfm/pose/util.h"
#include "theia/sfm/triangulation/triangulation.h"
#include "theia/sfm/types.h"

namespace theia {

using Eigen::Matrix3d;
using Eigen::Vector3d;

// Decomposes the essential matrix into the rotation R and translation t such
// that E can be any of the four candidate solutions: [rotation1 | translation],
// [rotation1 | -translation], [rotation2 | translation], [rotation2 |
// -translation].
void DecomposeEssentialMatrix(const Matrix3d& essential_matrix,
                              Matrix3d* rotation1,
                              Matrix3d* rotation2,
                              Vector3d* translation) {
  Matrix3d d;
  d << 0, 1, 0, -1, 0, 0, 0, 0, 1;

  const Eigen::JacobiSVD<Matrix3d> svd(
      essential_matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3d U = svd.matrixU();
  Eigen::Matrix3d V = svd.matrixV();
  if (U.determinant() < 0) {
    U.col(2) *= -1.0;
  }

  if (V.determinant() < 0) {
    V.col(2) *= -1.0;
  }

  // Possible configurations.
  *rotation1 = U * d * V.transpose();
  *rotation2 = U * d.transpose() * V.transpose();
  *translation = U.col(2).normalized();
}

// x = (R2 * X + t2)
// x = (R2 * R1^t * X + t2)
// x = (R2 * (R1^t * X - t1) + t2)
// x = R2 * R1^t * X - R2 * t1 + t2
void EssentialMatrixFromTwoProjectionMatrices(
    const Matrix3x4d& pose1,
    const Matrix3x4d& pose2,
    Eigen::Matrix3d* essential_matrix) {
  // Create the Ematrix from the poses.
  const Eigen::Matrix3d R1 = pose1.leftCols<3>();
  const Eigen::Matrix3d R2 = pose2.leftCols<3>();
  const Eigen::Vector3d t1 = pose1.rightCols<1>();
  const Eigen::Vector3d t2 = pose2.rightCols<1>();

  // Pos1 = -R1^t * t1.
  // Pos2 = -R2^t * t2.
  // t = R1 * (pos2 - pos1).
  // t = R1 * (-R2^t * t2 + R1^t * t1)
  // t = t1 - R1 * R2^t * t2;

  // Relative transformation between to cameras.
  const Eigen::Matrix3d relative_rotation = R1 * R2.transpose();
  const Eigen::Vector3d translation = (t1 - relative_rotation * t2).normalized();
  *essential_matrix = CrossProductMatrix(translation) * relative_rotation;
}

int GetBestPoseFromEssentialMatrix(
    const Matrix3d& essential_matrix,
    const std::vector<FeatureCorrespondence>& normalized_correspondences,
    Matrix3d* rotation,
    Vector3d* position) {
  // Decompose ematrix.
  Matrix3d rotation1, rotation2;
  Vector3d translation;
  DecomposeEssentialMatrix(
      essential_matrix, &rotation1, &rotation2, &translation);
  const std::vector<Matrix3d> rotations = {
      rotation1, rotation1, rotation2, rotation2};
  const std::vector<Vector3d> positions = {
      -rotations[0].transpose() * translation,
      -rotations[1].transpose() * -translation,
      -rotations[2].transpose() * translation,
      -rotations[3].transpose() * -translation};

  // From the 4 candidate poses, find the one with the most triangulated points
  // in front of the camera.
  std::vector<int> points_in_front_of_cameras(4, 0);
  for (int i = 0; i < 4; i++) {
    for (const auto& correspondence : normalized_correspondences) {
      if (IsTriangulatedPointInFrontOfCameras(
              correspondence, rotations[i], positions[i])) {
        ++points_in_front_of_cameras[i];
      }
    }
  }

  // Find the pose with the most points in front of the camera.
  const auto& max_element = std::max_element(points_in_front_of_cameras.begin(),
                                             points_in_front_of_cameras.end());
  const int max_index =
      std::distance(points_in_front_of_cameras.begin(), max_element);

  // Set the pose.
  *rotation = rotations[max_index];
  *position = positions[max_index];
  return *max_element;
}

}  // namespace theia


#include <boost/algorithm/string/classification.hpp>
#include <boost/algorithm/string/split.hpp>
#include <boost/program_options.hpp>
#include <cmath>
#include <chrono>
#include <iostream>
#include <is/is.hpp>
#include <is/msgs/common.hpp>
#include <is/msgs/robot.hpp>

using namespace is::msg::robot;
using namespace is::msg::common;
using namespace std::chrono_literals;
namespace po = boost::program_options;

constexpr double pi() {
  return std::atan(1) * 4;
}
auto deg2rad = [](double deg) { return deg * (pi() / 180.0); };
auto rad2deg = [](double rad) { return rad * (180.0 / pi()); };

int main(int argc, char* argv[]) {
  std::string uri;
  std::string entity;
  std::string pose_str;
  double v, w;
  int64_t period;

  po::options_description description("Allowed options");
  auto&& options = description.add_options();
  options("help,", "show available options");
  options("uri,u", po::value<std::string>(&uri)->default_value("amqp://localhost"), "broker uri");
  options("entity,e", po::value<std::string>(&entity)->default_value("pioneer.0"), "gateway name");
  options("linvel,v", po::value<double>(&v)->default_value(100.0), "linear velocity in mm/s");
  options("rotvel,w", po::value<double>(&w)->default_value(5.0), "rotational velocity in deg/s");
  options("initial_pose,p", po::value<std::string>(&pose_str)->default_value("0.0,0.0,0.0"),
          "initial pose (x[mm],y[mm],th[deg]) e.g.: 0.0,100.0,30");
  options("period,t", po::value<int64_t>(&period)->default_value(100), "sampling period in ms");

  po::variables_map vm;
  po::store(po::parse_command_line(argc, argv, description), vm);
  po::notify(vm);

  if (vm.count("help") || !vm.count("entity")) {
    std::cout << description << std::endl;
    return 1;
  }

  std::cout << pose_str << std::endl;

  auto is = is::connect(uri);
  auto client = is::make_client(is);

  if (vm.count("initial_pose")) {
    std::vector<std::string> fields;
    boost::split(fields, pose_str, boost::is_any_of(","), boost::token_compress_on);
    if (fields.size() == 3) {
      Pose pose{.position = {stod(fields[0]), stod(fields[1])}, .heading = deg2rad(stod(fields[2]))};
      client.request(entity + ".set_pose", is::msgpack(pose));
    }
  }

  if (vm.count("linvel") && vm.count("rotvel")) {
    Speed speed{v, deg2rad(w)};
    client.request(entity + ".set_speed", is::msgpack(speed));
  }

  if (vm.count("period")) {
    SamplingRate sample_rate;
    sample_rate.period = period;
    client.request(entity + ".set_sample_rate", is::msgpack(sample_rate));
  }

  while (client.receive_for(1s) != nullptr) {
  }

  auto poses = is.subscribe({entity + ".pose"});
  auto timestamps = is.subscribe({entity + ".timestamp"});

  int i = 10;
  while (1) {
    if (i++ % 10 == 0) {
      is::logger()->info("[ns] [mm], [mm], [deg]");
    }

    auto pose_msg = is.consume(poses);
    auto timestamp_msg = is.consume(timestamps);

    auto pose = is::msgpack<Pose>(pose_msg);
    auto timestamp = is::msgpack<Timestamp>(timestamp_msg);

    is::logger()->info("[{}] {}, {}, {}", timestamp.nanoseconds, pose.position.x, pose.position.y,
                       rad2deg(pose.heading));
  }

  return 0;
}