#include <boost/multiprecision/cpp_int.hpp>
#include <peersafe/crypto/LibSnark.h>
#include <peersafe/precompiled/PreContractRegister.h>
#include <peersafe/basics/TypeTransform.h>
#include <ripple/protocol/digest.h>
#include <ripple/protocol/CommonKey.h>
#include <ripple/basics/Blob.h>
#include <ripple/basics/Slice.h>
using namespace std;

namespace ripple {

PrecompiledRegistrar* PrecompiledRegistrar::s_this = nullptr;

PrecompiledExecutor const& PrecompiledRegistrar::executor(std::string const& _name)
{
    if (!get()->m_execs.count(_name))
        BOOST_THROW_EXCEPTION(ExecutorNotFound());
    return get()->m_execs[_name];
}

PrecompiledPricer const& PrecompiledRegistrar::pricer(std::string const& _name)
{
    if (!get()->m_pricers.count(_name))
        BOOST_THROW_EXCEPTION(PricerNotFound());
    return get()->m_pricers[_name];
}

namespace
{

// // Big-endian to/from host endian conversion functions.

// /// Converts a templated integer value to the big-endian byte-stream represented on a templated collection.
// /// The size of the collection object will be unchanged. If it is too small, it will not represent the
// /// value properly, if too big then the additional elements will be zeroed out.
// /// @a Out will typically be either std::string or bytes.
// /// @a T will typically by unsigned, u160, u256 or bigint.
// template <class T, class Out>
// inline void toBigEndian(T _val, Out& o_out)
// {
// 	static_assert(std::is_same<eth::bigint, T>::value || !std::numeric_limits<T>::is_signed, "only unsigned types or bigint supported"); //bigint does not carry sign bit on shift
// 	for (auto i = o_out.size(); i != 0; _val >>= 8, i--)
// 	{
// 		T v = _val & (T)0xff;
// 		o_out[i - 1] = (typename Out::value_type)(uint8_t)v;
// 	}
// }

// /// Converts a big-endian byte-stream represented on a templated collection to a templated integer value.
// /// @a _In will typically be either std::string or bytes.
// /// @a T will typically by unsigned, u160, u256 or bigint.
// template <class T, class _In>
// inline T fromBigEndian(_In const& _bytes)
// {
// 	T ret = (T)0;
// 	for (auto i: _bytes)
// 		ret = (T)((ret << 8) | (eth::byte)(typename std::make_unsigned<decltype(i)>::type)i);
// 	return ret;
// }

int64_t linearPricer(unsigned _base, unsigned _word, eth::bytesConstRef _in)
{
    int64_t const s = _in.size();
    int64_t const b = _base;
    int64_t const w = _word;
    return b + (s + 31) / 32 * w;
}

// ETH_REGISTER_PRECOMPILED_PRICER(ecrecover)
// (eth::bytesConstRef /*_in*/, ChainOperationParams const& /*_chainParams*/, int64_t const& /*_blockNumber*/)
// {
//     return 3000;
// }

// ETH_REGISTER_PRECOMPILED(ecrecover)(eth::bytesConstRef _in)
// {
//     struct
//     {
//         h256 hash;
//         h256 v;
//         h256 r;
//         h256 s;
//     } in;

//     memcpy(&in, _in.data(), min(_in.size(), sizeof(in)));

//     h256 ret;
//     u256 v = (u256)in.v;
//     if (v >= 27 && v <= 28)
//     {
//         SignatureStruct sig(in.r, in.s, (byte)((int)v - 27));
//         if (sig.isValid())
//         {
//             try
//             {
//                 if (Public rec = recover(sig, in.hash))
//                 {
//                     ret = dev::sha3(rec);
//                     memset(ret.data(), 0, 12);
//                     return {true, ret.asBytes()};
//                 }
//             }
//             catch (...) {}
//         }
//     }
//     return {true, {}};
// }

ETH_REGISTER_PRECOMPILED_PRICER(sha256)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(sha256)(eth::bytesConstRef _in)
{
    auto hashRet = sha512Half<CommonKey::sha>(Slice(_in.data(), _in.size()));
    return {true, Blob(hashRet.begin(), hashRet.end())};
}

ETH_REGISTER_PRECOMPILED_PRICER(sm3)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(sm3)(eth::bytesConstRef _in)
{
    auto hashRet = sha512Half<CommonKey::sm3>(Slice(_in.data(), _in.size()));
    return {true, Blob(hashRet.begin(), hashRet.end())};
}

ETH_REGISTER_PRECOMPILED_PRICER(ripemd160)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(600, 120, _in);
}

ETH_REGISTER_PRECOMPILED(ripemd160)(eth::bytesConstRef _in)
{
    // return {true, h256(dev::ripemd160(_in), h256::AlignRight).asBytes()};
    ripemd160_hasher rh;
    rh(_in.data(), _in.size());
    auto rhRet = ripemd160_hasher::result_type(rh);
    return {true, Blob(rhRet.begin(), rhRet.end())};
}

ETH_REGISTER_PRECOMPILED_PRICER(identity)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(15, 3, _in);
}

ETH_REGISTER_PRECOMPILED(identity)(eth::bytesConstRef _in)
{
    return {true, _in.toBytes()};
}

ETH_REGISTER_PRECOMPILED_PRICER(enbase58)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(enbase58)(eth::bytesConstRef _in/*, std::uint8_t tokenType = (std::uint8_t)TokenType::AccountID*/)
{
    auto enRet = base58EncodeToken(TokenType::AccountID, _in.data(), _in.size());
    return { true, Blob(enRet.begin(), enRet.end()) };
}

ETH_REGISTER_PRECOMPILED_PRICER(debase58)
(eth::bytesConstRef _in, int64_t const& /*_blockNumber*/)
{
    return linearPricer(60, 12, _in);
}

ETH_REGISTER_PRECOMPILED(debase58)(eth::bytesConstRef _in/*, std::uint8_t tokenType = (std::uint8_t)TokenType::AccountID*/)
{
    auto enRet = decodeBase58Token(std::string((const char*)_in.data(), _in.size()), TokenType::AccountID);
    return { true, Blob(enRet.begin(), enRet.end()) };
}

// Parse _count bytes of _in starting with _begin offset as big endian int.
// If there's not enough bytes in _in, consider it infinitely right-padded with zeroes.
eth::bigint parseBigEndianRightPadded(eth::bytesConstRef _in, eth::bigint const& _begin, eth::bigint const& _count)
{
    if (_begin > _in.count())
        return 0;
    assert(_count <= numeric_limits<size_t>::max() / 8); // Otherwise, the return value would not fit in the memory.

    size_t const begin{_begin};
    size_t const count{_count};

    // crop _in, not going beyond its size
    eth::bytesConstRef cropped = _in.cropped(begin, min(count, _in.count() - begin));

    eth::bigint ret = ripple::fromBigEndian<eth::bigint>(cropped);
    // shift as if we had right-padding zeroes
    assert(count - cropped.count() <= numeric_limits<size_t>::max() / 8);
    ret <<= 8 * (count - cropped.count());

    return ret;
}

 ETH_REGISTER_PRECOMPILED(modexp)(eth::bytesConstRef _in)
 {
     eth::bigint const baseLength(parseBigEndianRightPadded(_in, 0, 32));
     eth::bigint const expLength(parseBigEndianRightPadded(_in, 32, 32));
     eth::bigint const modLength(parseBigEndianRightPadded(_in, 64, 32));
     assert(modLength <= numeric_limits<size_t>::max() / 8); // Otherwise gas should be too expensive.
     assert(baseLength <= numeric_limits<size_t>::max() / 8); // Otherwise, gas should be too expensive.
     if (modLength == 0 && baseLength == 0)
         return {true, eth::bytes{}}; // This is a special case where expLength can be very big.
     assert(expLength <= numeric_limits<size_t>::max() / 8);

     eth::bigint const base(parseBigEndianRightPadded(_in, 96, baseLength));
     eth::bigint const exp(parseBigEndianRightPadded(_in, 96 + baseLength, expLength));
     eth::bigint const mod(parseBigEndianRightPadded(_in, 96 + baseLength + expLength, modLength));

     eth::bigint const result = mod != 0 ? boost::multiprecision::powm(base, exp, mod) : eth::bigint{0};

     size_t const retLength(modLength);
     eth::bytes ret(retLength);
     ripple::toBigEndian(result, ret);

     return {true, ret};
 }

 namespace
 {
     int64_t expLengthAdjust(int64_t const& _expOffset, int64_t const& _expLength, eth::bytesConstRef _in)
     {
         if (_expLength <= 32)
         {
             eth::bigint const exp(parseBigEndianRightPadded(_in, _expOffset, _expLength));
             return exp ? boost::multiprecision::msb(exp) : 0;
         }
         else
         {
             eth::bigint const expFirstWord(parseBigEndianRightPadded(_in, _expOffset, 32));
             size_t const highestBit(expFirstWord ? boost::multiprecision::msb(expFirstWord) : 0);
             return 8 * (_expLength - 32) + highestBit;
         }
     }

     int64_t multComplexity(int64_t const& _x)
     {
         if (_x <= 64)
             return _x * _x;
         if (_x <= 1024)
             return (_x * _x) / 4 + 96 * _x - 3072;
         else
             return (_x * _x) / 16 + 480 * _x - 199680;
     }
 }

 ETH_REGISTER_PRECOMPILED_PRICER(modexp)(eth::bytesConstRef _in, int64_t const&)
 {
     int64_t const baseLength(parseBigEndianRightPadded(_in, 0, 32));
     int64_t const expLength(parseBigEndianRightPadded(_in, 32, 32));
     int64_t const modLength(parseBigEndianRightPadded(_in, 64, 32));

     int64_t const maxLength(max(modLength, baseLength));
     int64_t const adjustedExpLength(expLengthAdjust(baseLength + 96, expLength, _in));

     return multComplexity(maxLength) * max<int64_t>(adjustedExpLength, 1) / 20;
 }

 ETH_REGISTER_PRECOMPILED(alt_bn128_G1_add)(eth::bytesConstRef _in)
 {
     return peersafe::alt_bn128_G1_add(_in);
 }

 ETH_REGISTER_PRECOMPILED_PRICER(alt_bn128_G1_add)
 (eth::bytesConstRef /*_in*/, int64_t const& _blockNumber)
 {
     return 150;
     // return _blockNumber < _chainParams.istanbulForkBlock ? 500 : 150;
 }

 ETH_REGISTER_PRECOMPILED(alt_bn128_G1_mul)(eth::bytesConstRef _in)
 {
     return  peersafe::alt_bn128_G1_mul(_in);
 }

 ETH_REGISTER_PRECOMPILED_PRICER(alt_bn128_G1_mul)
 (eth::bytesConstRef /*_in*/, int64_t const& _blockNumber)
 {
     return 6000;
     // return _blockNumber < _chainParams.istanbulForkBlock ? 40000 : 6000;
 }

 ETH_REGISTER_PRECOMPILED(alt_bn128_pairing_product)(eth::bytesConstRef _in)
 {
     return  peersafe::alt_bn128_pairing_product(_in);
 }

 ETH_REGISTER_PRECOMPILED_PRICER(alt_bn128_pairing_product)
 (eth::bytesConstRef _in, int64_t const& _blockNumber)
 {
     auto const k = _in.size() / 192;
     return 45000 + k * 34000;
     // return _blockNumber < _chainParams.istanbulForkBlock ? 100000 + k * 80000 : 45000 + k * 34000;
 }

// ETH_REGISTER_PRECOMPILED(blake2_compression)(eth::bytesConstRef _in)
// {
//     static constexpr size_t roundsSize = 4;
//     static constexpr size_t stateVectorSize = 8 * 8;
//     static constexpr size_t messageBlockSize = 16 * 8;
//     static constexpr size_t offsetCounterSize = 8;
//     static constexpr size_t finalBlockIndicatorSize = 1;
//     static constexpr size_t totalInputSize = roundsSize + stateVectorSize + messageBlockSize +
//                                              2 * offsetCounterSize + finalBlockIndicatorSize;

//     if (_in.size() != totalInputSize)
//         return {false, {}};

//     auto const rounds = fromBigEndian<uint32_t>(_in.cropped(0, roundsSize));
//     auto const stateVector = _in.cropped(roundsSize, stateVectorSize);
//     auto const messageBlockVector = _in.cropped(roundsSize + stateVectorSize, messageBlockSize);
//     auto const offsetCounter0 =
//         _in.cropped(roundsSize + stateVectorSize + messageBlockSize, offsetCounterSize);
//     auto const offsetCounter1 = _in.cropped(
//         roundsSize + stateVectorSize + messageBlockSize + offsetCounterSize, offsetCounterSize);
//     uint8_t const finalBlockIndicator =
//         _in[roundsSize + stateVectorSize + messageBlockSize + 2 * offsetCounterSize];

//     if (finalBlockIndicator != 0 && finalBlockIndicator != 1)
//         return {false, {}};

//     return {true, dev::crypto::blake2FCompression(rounds, stateVector, offsetCounter0,
//                       offsetCounter1, finalBlockIndicator, messageBlockVector)};
// }

// ETH_REGISTER_PRECOMPILED_PRICER(blake2_compression)
// (eth::bytesConstRef _in, int64_t const&)
// {
//     auto const rounds = fromBigEndian<uint32_t>(_in.cropped(0, 4));
//     return rounds;
// }
}

}


/* Copyright © 2017 Apple Inc. All rights reserved.
 *
 * Use of this source code is governed by a BSD-3-clause license that can
 * be found in the LICENSE.txt file or at https://opensource.org/licenses/BSD-3-Clause
 */
// ML Data
#include <sframe/sframe.hpp>
#include <unity/lib/unity_sframe.hpp>
#include <unity/lib/variant_deep_serialize.hpp>
;
#include <sframe/algorithm.hpp>
#include <ml_data/ml_data.hpp>

// Toolkits
#include <unity/toolkits/supervised_learning/supervised_learning.hpp>
#include <unity/toolkits/supervised_learning/supervised_learning_utils-inl.hpp>
#include <unity/toolkits/supervised_learning/linear_svm.hpp>

// Solvers
#include <optimization/utils.hpp>
#include <optimization/constraints-inl.hpp>
#include <limits>

// Regularizer
#include <optimization/regularizers-inl.hpp>
#include <optimization/lbfgs.hpp>
#include <optimization/newton_method-inl.hpp>
#include <optimization/accelerated_gradient-inl.hpp>

#include <unity/toolkits/supervised_learning/linear_svm.hpp>
#include <unity/toolkits/supervised_learning/linear_svm_opt_interface.hpp>

// Utilities
#include <Eigen/SparseCore>
#include <cmath>

namespace turi {
namespace supervised {

/*
* SVM supervised_learning Solver Interface
*
*******************************************************************************
*/

/**
* Constructor for linear svm solver object
*/
linear_svm_scaled_logistic_opt_interface::linear_svm_scaled_logistic_opt_interface(
    const ml_data& _data,
    const ml_data& _valid_data,
    linear_svm& _model) {

  // Initialize ml_data, readers, and other metadata.
  // -----------------------------------------------------------------------
  data = _data;
  if (_valid_data.num_rows() > 0) valid_data = _valid_data;
  smodel = _model;

  // Initialize reader and other data
  examples = data.num_rows();
  features = data.num_columns();
  n_threads = turi::thread_pool::get_instance().size();

  // Initialize the number of variables to 1 (bias term)
  primal_variables = get_number_of_coefficients(smodel.get_ml_metadata());
  is_dense = (primal_variables <= 3 * features) ? true : false;
}

/**
* Set gamma
*/
void linear_svm_scaled_logistic_opt_interface::set_gamma(const double _gamma){
  gamma = _gamma;
}

/**
* Destructor for the linear svm solver object
*/
linear_svm_scaled_logistic_opt_interface::~linear_svm_scaled_logistic_opt_interface() {
}

/**
* Set the class weights (as a flex_dict which is already validated)
*/
void linear_svm_scaled_logistic_opt_interface::set_class_weights(
                                        const flexible_type& _class_weights) {
  DASSERT_TRUE(_class_weights.size() == classes);
  for(const auto& kvp: _class_weights.get<flex_dict>()){
    class_weights[kvp.first.get<flex_int>()]= kvp.second.get<flex_float>();
  }
}

/**
* Set the number of threads
*/
void linear_svm_scaled_logistic_opt_interface::set_threads(size_t _n_threads) {
  n_threads = _n_threads;
}

/**
* Get the number of examples for the model
*/
size_t linear_svm_scaled_logistic_opt_interface::num_examples() const{
  return examples;
}

/**
* Get the number of validation-set examples for the model
*/
size_t linear_svm_scaled_logistic_opt_interface::num_validation_examples() const{
  return valid_data.num_rows();
}

/**
* Get the number of variables for the model
*/
size_t linear_svm_scaled_logistic_opt_interface::num_variables() const{
  return primal_variables;
}

/**
* Get the number of classes for the model
*/
size_t linear_svm_scaled_logistic_opt_interface::num_classes() const{
  return classes;
}

/**
 * Get strings needed to print the header for the progress table.
 */
std::vector<std::pair<std::string, size_t>>
linear_svm_scaled_logistic_opt_interface::get_status_header(const std::vector<std::string>& stat_headers) {
  bool has_validation_data = (valid_data.num_rows() > 0);
  auto header = make_progress_header(smodel, stat_headers, has_validation_data);
  return header;
}

/**
 * Get strings needed to print a row of the progress table.
 */
std::vector<std::string> linear_svm_scaled_logistic_opt_interface::get_status(
    const DenseVector& coefs,
    const std::vector<std::string>& stats) {

  // Copy coefficients, rescale, and update the model.
  DenseVector coefs_tmp = coefs;
  rescale_solution(coefs_tmp);
  smodel.set_coefs(coefs_tmp);

  auto ret = make_progress_row_string(smodel, data, valid_data, stats);
  return ret;
}

/**
 * Set feature rescaling.
 */
void linear_svm_scaled_logistic_opt_interface::init_feature_rescaling() {
  feature_rescaling = true;
  scaler.reset(new l2_rescaling(data.metadata(), true));
}

/**
 * Transform final solution back to the original scale.
 */
void linear_svm_scaled_logistic_opt_interface::rescale_solution(DenseVector& coefs) {
  if(feature_rescaling){
    scaler->transform(coefs);
  }
}

double linear_svm_scaled_logistic_opt_interface::get_validation_accuracy() {
  DASSERT_TRUE(valid_data.num_rows() > 0);

  auto eval_results = smodel.evaluate(valid_data, "train");
  auto results = eval_results.find("accuracy");
  if(results == eval_results.end()) {
    log_and_throw("No Validation Accuracy.");
  }

  variant_type variant_accuracy = results->second;
  double accuracy = variant_get_value<flexible_type>(variant_accuracy).to<double>();
  return accuracy;
}

double linear_svm_scaled_logistic_opt_interface::get_training_accuracy() {
  auto eval_results = smodel.evaluate(data, "train");
  auto results = eval_results.find("accuracy");

  if(results == eval_results.end()) {
    log_and_throw("No Validation Accuracy.");
  }
  variant_type variant_accuracy = results->second;
  double accuracy = variant_get_value<flexible_type>(variant_accuracy).to<double>();

  return accuracy;
}

/**
 * Compute the first order statistics
*/
void linear_svm_scaled_logistic_opt_interface::compute_first_order_statistics(const
    DenseVector& point, DenseVector& gradient, double& function_value, const
    size_t mbStart, const size_t mbSize) {

  // Mini-batch gradient is meaningless in the dual space.
  DASSERT_EQ(mbStart, 0);
  DASSERT_EQ(mbSize, (size_t)(-1));
  DASSERT_EQ(point.size(), primal_variables);

  // Init
  std::vector<double> f(n_threads, 0.0);
  std::vector<DenseVector> G(n_threads, Eigen::MatrixXd::Zero(primal_variables, 1));

  // Dense data.
  if (this->is_dense) {
    in_parallel([&](size_t thread_idx, size_t num_threads) {
      DenseVector x(primal_variables);
      double y, row_prob, margin, row_func;
      size_t class_idx = 0;
      for(auto it = data.get_iterator(thread_idx, num_threads);
                                                              !it.done(); ++it) {
        fill_reference_encoding(*it, x);
        x.coeffRef(primal_variables - 1) = 1;
        if(feature_rescaling){
          scaler->transform(x);
        }

        // Map
        class_idx = it->target_index();
        y = class_idx * 2 - 1.0;
        margin = -gamma * (y * x.dot(point) - 1);
        row_prob = -1.0/(1 + exp(-margin));
        if (margin < -100){
          row_func = 0;
        } else if (margin > 50){
          row_func = margin;
        } else {
          row_func = log1p(exp(margin));
        }
        f[thread_idx] += class_weights[class_idx] * row_func / gamma;
        G[thread_idx] += class_weights[class_idx] * y * x * row_prob;
      }
    });

  // Sparse data
  } else {
    in_parallel([&](size_t thread_idx, size_t num_threads) {
      SparseVector x(primal_variables);
      double y, row_prob, margin, row_func;
      size_t class_idx = 0;
      for(auto it = data.get_iterator(thread_idx, num_threads);
                                                              !it.done(); ++it) {
        fill_reference_encoding(*it, x);
        x.coeffRef(primal_variables - 1) = 1;
        if(feature_rescaling){
          scaler->transform(x);
        }

        // Map
        class_idx = it->target_index();
        y = class_idx * 2 - 1.0;
        margin = -gamma * (y * x.dot(point) - 1);
        row_prob = -1.0/(1 + exp(-margin));
        if (margin < -100){
          row_func = 0;
        } else if (margin > 50){
          row_func = margin;
        } else {
          row_func = log1p(exp(margin));
        }
        f[thread_idx] += class_weights[class_idx] * row_func / gamma;
        SparseVector G_tmp = class_weights[class_idx] * y * x * row_prob;
        optimization::vector_add<DenseVector, SparseVector>(G[thread_idx], G_tmp);
      }
    });
  }

  // Reduce
  function_value = f[0];
  gradient = G[0];
  for(size_t i=1; i < n_threads; i++){
    function_value += f[i];
    gradient += G[i];
  }
}

} // supervised
} // turicreate


//
// Copyright 2005-2007 Adobe Systems Incorporated
//
// Distributed under the Boost Software License, Version 1.0
// See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt
//
#ifndef BOOST_GIL_EXTENSION_NUMERIC_CHANNEL_NUMERIC_OPERATIONS_HPP
#define BOOST_GIL_EXTENSION_NUMERIC_CHANNEL_NUMERIC_OPERATIONS_HPP

#include <boost/gil/channel.hpp>

namespace boost { namespace gil {

// Function objects and utilities for channel-wise numeric operations.
//
// List of currently defined functors:
//    channel_plus_t (+)
//    channel_minus_t (-)
//    channel_multiplies_t (*)
//    channel_divides_t (/),
//    channel_plus_scalar_t (+s)
//    channel_minus_scalar_t (-s),
//    channel_multiplies_scalar_t (*s)
//    channel_divides_scalar_t (/s),
//    channel_halves_t (/=2)
//    channel_zeros_t (=0)
//    channel_assigns_t (=)

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of addition of two channel values.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel1, typename Channel2, typename ChannelResult>
struct channel_plus_t
{
    using ChannelRef1 = typename channel_traits<Channel1>::const_reference;
    using ChannelRef2 = typename channel_traits<Channel2>::const_reference;
    static_assert(std::is_convertible<ChannelRef1, ChannelResult>::value,
        "ChannelRef1 not convertible to ChannelResult");
    static_assert(std::is_convertible<ChannelRef2, ChannelResult>::value,
        "ChannelRef2 not convertible to ChannelResult");

    /// \param ch1 - first of the two addends (augend).
    /// \param ch2 - second of the two addends.
    auto operator()(ChannelRef1 ch1, ChannelRef2 ch2) const -> ChannelResult
    {
        return ChannelResult(ch1) + ChannelResult(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of subtraction of two channel values.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel1, typename Channel2, typename ChannelResult>
struct channel_minus_t
{
    using ChannelRef1 = typename channel_traits<Channel1>::const_reference;
    using ChannelRef2 = typename channel_traits<Channel2>::const_reference;
    static_assert(std::is_convertible<ChannelRef1, ChannelResult>::value,
        "ChannelRef1 not convertible to ChannelResult");
    static_assert(std::is_convertible<ChannelRef2, ChannelResult>::value,
        "ChannelRef2 not convertible to ChannelResult");

    /// \param ch1 - minuend operand of the subtraction.
    /// \param ch2 - subtrahend operand of the subtraction.
    auto operator()(ChannelRef1 ch1, ChannelRef2 ch2) const -> ChannelResult
    {
        return ChannelResult(ch1) - ChannelResult(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of multiplication of two channel values.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel1, typename Channel2, typename ChannelResult>
struct channel_multiplies_t
{
    using ChannelRef1 = typename channel_traits<Channel1>::const_reference;
    using ChannelRef2 = typename channel_traits<Channel2>::const_reference;
    static_assert(std::is_convertible<ChannelRef1, ChannelResult>::value,
        "ChannelRef1 not convertible to ChannelResult");
    static_assert(std::is_convertible<ChannelRef2, ChannelResult>::value,
        "ChannelRef2 not convertible to ChannelResult");

    /// \param ch1 - first of the two factors (multiplicand).
    /// \param ch2 - second of the two factors (multiplier).
    auto operator()(ChannelRef1 ch1, ChannelRef2 ch2) const -> ChannelResult
    {
        return ChannelResult(ch1) * ChannelResult(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of division of two channel values.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel1, typename Channel2, typename ChannelResult>
struct channel_divides_t
{
    using ChannelRef1 = typename channel_traits<Channel1>::const_reference;
    using ChannelRef2 = typename channel_traits<Channel2>::const_reference;
    static_assert(std::is_convertible<ChannelRef1, ChannelResult>::value,
        "ChannelRef1 not convertible to ChannelResult");
    static_assert(std::is_convertible<ChannelRef2, ChannelResult>::value,
        "ChannelRef2 not convertible to ChannelResult");

    /// \param ch1 - dividend operand of the two division operation.
    /// \param ch2 - divisor operand of the two division operation.
    auto operator()(ChannelRef1 ch1, ChannelRef2 ch2) const -> ChannelResult
    {
        return ChannelResult(ch1) / ChannelResult(ch2);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of adding scalar to channel value.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel, typename Scalar, typename ChannelResult>
struct channel_plus_scalar_t
{
    using ChannelRef = typename channel_traits<Channel>::const_reference;
    static_assert(std::is_convertible<ChannelRef, ChannelResult>::value,
        "ChannelRef not convertible to ChannelResult");
    static_assert(std::is_scalar<Scalar>::value, "Scalar not a scalar");
    static_assert(std::is_convertible<Scalar, ChannelResult>::value,
        "Scalar not convertible to ChannelResult");

    auto operator()(ChannelRef channel, Scalar const& scalar) const -> ChannelResult
    {
        return ChannelResult(channel) + ChannelResult(scalar);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of subtracting scalar from channel value.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel, typename Scalar, typename ChannelResult>
struct channel_minus_scalar_t
{
    using ChannelRef = typename channel_traits<Channel>::const_reference;
    static_assert(std::is_convertible<ChannelRef, ChannelResult>::value,
        "ChannelRef not convertible to ChannelResult");
    static_assert(std::is_scalar<Scalar>::value, "Scalar not a scalar");
    static_assert(std::is_convertible<Scalar, ChannelResult>::value,
        "Scalar not convertible to ChannelResult");

    /// \param channel - minuend operand of the subtraction.
    /// \param scalar - subtrahend operand of the subtraction.
    auto operator()(ChannelRef channel, Scalar const& scalar) const -> ChannelResult
    {
        // TODO: Convertion after subtraction vs conversion of operands in channel_minus_t?
        return ChannelResult(channel - scalar);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of channel value by a scalar.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel, typename Scalar, typename ChannelResult>
struct channel_multiplies_scalar_t
{
    using ChannelRef = typename channel_traits<Channel>::const_reference;
    static_assert(std::is_convertible<ChannelRef, ChannelResult>::value,
        "ChannelRef not convertible to ChannelResult");
    static_assert(std::is_scalar<Scalar>::value, "Scalar not a scalar");
    static_assert(std::is_convertible<Scalar, ChannelResult>::value,
        "Scalar not convertible to ChannelResult");

    /// \param channel - first of the two factors (multiplicand).
    /// \param scalar - second of the two factors (multiplier).
    auto operator()(ChannelRef channel, Scalar const& scalar) const -> ChannelResult
    {
        return ChannelResult(channel) * ChannelResult(scalar);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of dividing channel value by scalar.
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel, typename Scalar, typename ChannelResult>
struct channel_divides_scalar_t
{
    using ChannelRef = typename channel_traits<Channel>::const_reference;
    static_assert(std::is_convertible<ChannelRef, ChannelResult>::value,
        "ChannelRef not convertible to ChannelResult");
    static_assert(std::is_scalar<Scalar>::value, "Scalar not a scalar");
    static_assert(std::is_convertible<Scalar, ChannelResult>::value,
        "Scalar not convertible to ChannelResult");

    /// \param channel - dividend operand of the two division operation.
    /// \param scalar - divisor operand of the two division operation.
    auto operator()(ChannelRef channel, Scalar const& scalar) const -> ChannelResult
    {
        return ChannelResult(channel) / ChannelResult(scalar);
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Arithmetic operation of dividing channel value by 2
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel>
struct channel_halves_t
{
    using ChannelRef = typename channel_traits<Channel>::reference;

    auto operator()(ChannelRef channel) const -> ChannelRef
    {
        // TODO: Split into steps: extract with explicit conversion to double, divide and assign?
        //double const v = ch;
        //ch = static_cast<Channel>(v / 2.0);
        channel /= 2.0;
        return channel;
    }
};

/// \ingroup ChannelNumericOperations
/// \brief Operation of setting channel value to zero
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel>
struct channel_zeros_t
{
    using ChannelRef = typename channel_traits<Channel>::reference;

    auto operator()(ChannelRef channel) const -> ChannelRef
    {
        channel = Channel(0);
        return channel;
    }
};

/// \ingroup ChannelNumericOperations
/// structure for assigning one channel to another
/// \note This is a generic implementation; user should specialize it for better performance.
template <typename Channel1, typename Channel2>
struct channel_assigns_t
{
    using ChannelRef1 = typename channel_traits<Channel1>::const_reference;
    using ChannelRef2 = typename channel_traits<Channel2>::reference;
    static_assert(std::is_convertible<ChannelRef1, Channel2>::value,
        "ChannelRef1 not convertible to Channel2");

    /// \param ch1 - assignor side (input) of the assignment operation
    /// \param ch2 - assignee side (output) of the assignment operation
    auto operator()(ChannelRef1 ch1, ChannelRef2 ch2) const -> ChannelRef2
    {
        ch2 = Channel2(ch1);
        return ch2;
    }
};

}}  // namespace boost::gil

#endif


//==================================================================================================
/**
  Copyright 2016 Numscale SAS

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

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/gammaln.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  BOOST_DISPATCH_OVERLOAD ( gammaln_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0 ) const BOOST_NOEXCEPT
    {
      return {bs::gammaln(a0[0]), bs::gammaln(a0[1])};

    }
  };

} } }

#endif


#include <Eigen/Eigen>
#include <stdlib.h>

#include "romeo_moveit_actions/metablock.hpp"

MetaBlock::MetaBlock(const std::string name,
          const double start_x,
          const double start_y,
          const double start_z,
          const double orien_x,
          const double orien_y,
          const double orien_z,
          const double orien_w,
          const uint shapeType,
          const double size,
          const double size_r)
{
  this->name = name;
  this->size = size*size_r;

  //set position
  start_pose.position.x = start_x;
  start_pose.position.y = start_y;
  start_pose.position.z = start_z; //-0.13;//-0.11; //getTableHeight(-0.9); //from -0.11 to -0.1
  this->start_pose.orientation.x = orien_x;
  this->start_pose.orientation.y = orien_y;
  this->start_pose.orientation.z = orien_z;
  this->start_pose.orientation.w = orien_w;

  if (start_pose.position.y < 0)
    start_pose.orientation.y *= -1;

  goal_pose = start_pose;
  goal_pose.position.y = 0.14;
  if (start_pose.position.y < 0)
    goal_pose.position.y *= -1;

  //setshape
  if (shapeType == shape_msgs::SolidPrimitive::CYLINDER)
  {
    shape_msgs::SolidPrimitive shapeCylinder;
    shapeCylinder.type = shape_msgs::SolidPrimitive::CYLINDER;
    shapeCylinder.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
    shapeCylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = size*size_r;
    shapeCylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = size;
    this->shape = shapeCylinder;
  }
  else
  {
    shape_msgs::SolidPrimitive shapeBox;
    shapeBox.type = shape_msgs::SolidPrimitive::BOX;
    shapeBox.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
    shapeBox.dimensions[shape_msgs::SolidPrimitive::BOX_X] = size;
    shapeBox.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = size;
    shapeBox.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = size;
    this->shape = shapeBox;
  }
}

MetaBlock::MetaBlock(const std::string name,
          const ros::Time timestamp,
          const geometry_msgs::Pose start_pose,
          const uint shapeType,
          const double size,
          const double size_l)
{
  this->name = name;
  this->timestamp = timestamp;
  this->size = size;
  this->size_l = size_l;

  this->start_pose = start_pose;

  goal_pose = start_pose;
  goal_pose.position.x = 0.5;
  goal_pose.position.y = 0.18;
  if (start_pose.position.y < 0)
    goal_pose.position.y = -0.25;

  /*this->goal_pose.orientation.x = -1.0;
  this->goal_pose.orientation.y = 0.0;
  this->goal_pose.orientation.z = 0.0;
  this->goal_pose.orientation.w = 0.0;*/

  //setshape
  if (shapeType == shape_msgs::SolidPrimitive::CYLINDER)
  {
    shape_msgs::SolidPrimitive shapeCylinder;
    shapeCylinder.type = shape_msgs::SolidPrimitive::CYLINDER;
    shapeCylinder.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::CYLINDER>::value);
    shapeCylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_HEIGHT] = size_l;
    shapeCylinder.dimensions[shape_msgs::SolidPrimitive::CYLINDER_RADIUS] = size;
    this->shape = shapeCylinder;
  }
  else
  {
    shape_msgs::SolidPrimitive shapeBox;
    shapeBox.type = shape_msgs::SolidPrimitive::BOX;
    shapeBox.dimensions.resize(geometric_shapes::SolidPrimitiveDimCount<shape_msgs::SolidPrimitive::BOX>::value);
    shapeBox.dimensions[shape_msgs::SolidPrimitive::BOX_X] = size;
    shapeBox.dimensions[shape_msgs::SolidPrimitive::BOX_Y] = size;
    shapeBox.dimensions[shape_msgs::SolidPrimitive::BOX_Z] = size_l;
    this->shape = shapeBox;
  }
}

MetaBlock::MetaBlock(const std::string name,
          const ros::Time timestamp,
          const geometry_msgs::Pose start_pose,
          const shape_msgs::Mesh mesh,
          const object_recognition_msgs::ObjectType type)
{
  this->name = name;
  this->timestamp = timestamp;

  this->start_pose = start_pose;
  this->start_pose.orientation.x = -1.0;
  this->start_pose.orientation.y = 0.0;
  this->start_pose.orientation.z = 0.0;
  this->start_pose.orientation.w = 0.0;

  this->goal_pose = start_pose;
  this->goal_pose.position.x = 0.4;//0.47; //0.5;//
  this->goal_pose.position.y = 0.3;
  this->goal_pose.position.z = -0.05;
  this->goal_pose.orientation.x = -1.0;
  this->goal_pose.orientation.y = 0.0;
  this->goal_pose.orientation.z = 0.0;
  this->goal_pose.orientation.w = 0.0;

  this->mesh = mesh;
  this->type = type;
}

void MetaBlock::updatePose(const geometry_msgs::Pose start_pose)
{
  this->start_pose = start_pose;
}

void MetaBlock::setRndPose()
{
  this->start_pose.position.x = 0.35f + float(rand() % 150)/1000.0f; //[0.35;0.50]
  this->start_pose.position.y = float(rand() % 90)/100.0f - 0.45; //[-0.45;0.45]
  this->start_pose.position.z = -0.23f + (float(rand() % 230)/1000.0f); //[-0.23;0.00]
}


#include "dynet/nodes.h"
#include "dynet/exec.h"
#include "dynet/dynet.h"
#include "dynet/training.h"
#include "dynet/timing.h"
#include "dynet/rnn.h"
#include "dynet/gru.h"
#include "dynet/lstm.h"
#include "dynet/fast-lstm.h"
#include "dynet/dict.h"
#include "dynet/expr.h"

#include <iostream>
#include <fstream>
#include <sstream>
#include <type_traits>

#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/program_options.hpp>

#include "s2s/encdec.hpp"
#include "s2s/define.hpp"
#include "s2s/comp.hpp"
#include "s2s/metrics.hpp"

namespace s2s {

    void greedy_decode(const batch& one_batch, std::vector<std::vector<unsigned int > >& osent, encoder_decoder *encdec, dicts &d, const s2s_options &opts){
        //unsigned slen = sents.size();
        dynet::ComputationGraph cg;
        osent.push_back(std::vector<unsigned int>(one_batch.src.at(0).at(0).size(), d.target_start_id));
        std::vector<dynet::expr::Expression> i_enc = encdec->encoder(one_batch, cg);
        std::vector<dynet::expr::Expression> i_feed = encdec->init_feed(one_batch, cg);
        for (int t = 0; t < opts.max_length; ++t) {
            dynet::expr::Expression i_att_t = encdec->decoder_attention(cg, osent[t], i_feed[t], i_enc[0]);
            std::vector<dynet::expr::Expression> i_out_t = encdec->decoder_output(cg, i_att_t, i_enc[1]);
            i_feed.push_back(i_out_t[1]);
            Expression predict = softmax(i_out_t[0]);
            std::vector<dynet::Tensor> results = cg.incremental_forward(predict).batch_elems();
            std::vector<unsigned int> osent_col;
            for(unsigned int i = 0; i < results.size(); i++){
                auto output = as_vector(results.at(i));
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(osent.back().at(i) == d.target_end_id || t == opts.max_length - 1){
                    w_id = d.target_end_id;
                }else{
                    for(unsigned int j=0; j<output.size(); j++){
                        if(output[j] > w_prob){
                            w_id = j;
                            w_prob = output[j];
                        }
                    }
                }
                osent_col.push_back(w_id);
            }
            osent.push_back(osent_col);
            // end check
            unsigned int num_end = 0;
            for(const unsigned int w_id : osent_col){
                if(w_id == d.target_end_id){
                    num_end++;
                }
            }
            if(num_end == osent_col.size()){
                break;
            }
        }
    }

    void greedy_decode_vinyals(const batch& one_batch, std::vector<std::vector<unsigned int > >& osent, encoder_decoder *encdec, dicts &d, const s2s_options &opts){
        //unsigned slen = sents.size();
        dynet::ComputationGraph cg;
        std::vector<unsigned int> XX_count = one_batch.len_src;
        osent.push_back(std::vector<unsigned int>(one_batch.src.at(0).at(0).size(), d.target_start_id));
        std::vector<dynet::expr::Expression> i_enc = encdec->encoder(one_batch, cg);
        std::vector<dynet::expr::Expression> i_feed = encdec->init_feed(one_batch, cg);
        // skip start and end symbols from the count
        for(unsigned int i=0; i < XX_count.size(); i++){
            XX_count[i] -= 2;
        }
        for (unsigned int t = 0; t < opts.max_length; ++t) {
            dynet::expr::Expression i_att_t = encdec->decoder_attention(cg, osent[t], i_feed[t], i_enc[0]);
            std::vector<dynet::expr::Expression> i_out_t = encdec->decoder_output(cg, i_att_t, i_enc[1]);
            i_feed.push_back(i_out_t[1]);
            Expression predict = softmax(i_out_t[0]);
            std::vector<dynet::Tensor> results = cg.incremental_forward(predict).batch_elems();
            std::vector<unsigned int> osent_col;
            for(unsigned int i = 0; i < results.size(); i++){
                auto output = as_vector(results.at(i));
                unsigned int w_id = 0;
                float w_prob = -FLT_MAX;
                if(osent.back().at(i) == d.target_end_id || t == opts.max_length - 1){
                    w_id = d.target_end_id;
                }else if(XX_count.at(i) >= ((opts.max_length - t) - 1)){
                    w_id = d.d_trg.convert("XX");
                }else{
                    for(unsigned int j=0; j < output.size(); j++){
                        if(XX_count.at(i) > 0){
                            if(j != d.target_end_id){
                                if(output[j] > w_prob){
                                    w_id = j;
                                    w_prob = output[j];
                                }
                            }
                        }else if(XX_count.at(i) == 0){
                            std::string w_str = d.d_trg.convert(j);
                            if(j != d.d_trg.convert("XX") && w_str[0] != '('){
                                if(output[j] > w_prob){
                                    w_id = j;
                                    w_prob = output[j];
                                }
                            }
                        }else{
                            std::cerr << "Count does not match." << std::endl;
                            assert(false);
                        }
                    }
                }
                osent_col.push_back(w_id);
                if(w_id == d.d_trg.convert("XX")){
                    XX_count[i]--;
                }
            }
            osent.push_back(osent_col);
            // end check
            unsigned int num_end = 0;
            for(const unsigned int w_id : osent_col){
                if(w_id == d.target_end_id){
                    num_end++;
                }
            }
            if(num_end == osent_col.size()){
                break;
            }
        }
    }

    void beam_decode(){
    }

    std::vector<std::string> print_sents(std::vector<std::vector<unsigned int > >& osent, dicts& d){
        std::vector<std::string> str_sents;
        std::vector<std::vector<unsigned int> > sents_conved;
        sents_conved.resize(osent.at(0).size());
        for(unsigned int col_id = 0; col_id < osent.size(); col_id++){
            for(unsigned int sid = 0; sid < osent.at(col_id).size(); sid++){
                sents_conved[sid].push_back(osent.at(col_id).at(sid));
            }
        }
        for(const auto sent : sents_conved){
            std::string str_sent = "";
            for(const auto wid : sent){
                std::string word = d.d_trg.convert(wid);
                str_sent += word;
                if(wid == d.target_end_id){
                    break;
                }
                str_sent += " ";
            }
            str_sents.push_back(str_sent);
        }
        return str_sents;
    }

};


#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_eigen/tf2_eigen.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>

#include <geometry_msgs/TransformStamped.h>
#include <image_geometry/pinhole_camera_model.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <std_msgs/ColorRGBA.h>
#include <person_msgs/PersonCovList.h>
#include <person_msgs/Person2DList.h>
#include <skeleton_3d/fusion_body_parts.h>

#include <string>
#include <vector>
#include <map>
#include <functional>

using std::string;
using std::cout;
using std::endl;
using std::vector;
using std::map;
using person_msgs::PersonCovList;
using person_msgs::Person2DList;
using person_msgs::Person2D;

const string BASE_FRAME = "base";
const string g_cam_frame = "_color_optical_frame";
const string g_cam_info_topic = "/color/camera_info";
const string g_skel_pred_topic = "/skel_pred";
static vector<string> CAM_FRAMES = {"cam_1_color_optical_frame", "cam_2_color_optical_frame", "cam_3_color_optical_frame", "cam_4_color_optical_frame"};
static vector<string> CAM_INFO_TOPICS = {"cam_1/color/camera_info", "cam_2/color/camera_info", "cam_3/color/camera_info", "cam_4/color/camera_info"};
static vector<string> SKELETON_PRED_TOPICS = {"cam_1/skel_pred", "cam_2/skel_pred", "cam_3/skel_pred", "cam_4/skel_pred"};
static unsigned int NUM_CAMERAS = 4;

const string FUSED_SKELETON_TOPIC = "human_pose_estimation/persons3d_fused_pred";

static unsigned int NUM_KEYPOINTS = 17;

static string g_param_pose_method = "simple"; // "simple" or h36m

const int g_kp2kpFusion_idx_simple[17] = {FUSION_BODY_PARTS::Nose,
                                              FUSION_BODY_PARTS::LEye, FUSION_BODY_PARTS::REye, FUSION_BODY_PARTS::LEar, FUSION_BODY_PARTS::REar,
                                              FUSION_BODY_PARTS::LShoulder, FUSION_BODY_PARTS::RShoulder, FUSION_BODY_PARTS::LElbow, FUSION_BODY_PARTS::RElbow, FUSION_BODY_PARTS::LWrist, FUSION_BODY_PARTS::RWrist,
                                              FUSION_BODY_PARTS::LHip, FUSION_BODY_PARTS::RHip, FUSION_BODY_PARTS::LKnee, FUSION_BODY_PARTS::RKnee, FUSION_BODY_PARTS::LAnkle, FUSION_BODY_PARTS::RAnkle};
const int g_kp2kpFusion_idx_h36m[17] = {FUSION_BODY_PARTS::Nose, FUSION_BODY_PARTS::Head, FUSION_BODY_PARTS::Neck, FUSION_BODY_PARTS::Belly, FUSION_BODY_PARTS::MidHip,
                                              FUSION_BODY_PARTS::LShoulder, FUSION_BODY_PARTS::RShoulder, FUSION_BODY_PARTS::LElbow, FUSION_BODY_PARTS::RElbow, FUSION_BODY_PARTS::LWrist, FUSION_BODY_PARTS::RWrist,
                                              FUSION_BODY_PARTS::LHip, FUSION_BODY_PARTS::RHip, FUSION_BODY_PARTS::LKnee, FUSION_BODY_PARTS::RKnee, FUSION_BODY_PARTS::LAnkle, FUSION_BODY_PARTS::RAnkle};
static const int* g_kp2kpFusion_idx;

const int DIM = 3;
const int N_SAMPLES = 2*DIM+1;
typedef Eigen::Matrix<double, DIM, N_SAMPLES> SamplesMatType;
typedef Eigen::Matrix<double, 1, N_SAMPLES> SamplesWeightType;
typedef Eigen::Matrix<double, 2, N_SAMPLES> TransformedSampledMatType;

SamplesWeightType draw_sigma_points(SamplesMatType& sigma_points, Eigen::Vector3d mean, Eigen::Matrix3d cov){
  const double kappa = 0.5; // (scaling) equal weight on all samples // TODO: find best parameter (or use scaled transform, kappa=0, alpha = 1e-3, beta = 2)
  SamplesWeightType weights;
  weights << 2*kappa, Eigen::Matrix<double, 1, N_SAMPLES - 1>::Ones();
  weights /= (2.0 * (DIM + kappa));

  Eigen::Matrix3d mat = std::sqrt(DIM + kappa) * Eigen::Matrix3d::Identity();
  SamplesMatType samples_std_normal;
  samples_std_normal << Eigen::Vector3d::Zero(), -mat, mat;

  sigma_points = cov.llt().matrixL() * samples_std_normal + mean.replicate<1, N_SAMPLES>();

  return weights;
}

void getTransforms(map<string, geometry_msgs::TransformStamped>& transforms_cam, const tf2_ros::Buffer& tfBuffer){
  bool success = false;
  while (!success && ros::ok()){
    try {
      for (int i = 0; i < NUM_CAMERAS; ++i) {
        auto transform = tfBuffer.lookupTransform(CAM_FRAMES[i], BASE_FRAME, ros::Time(0));
        auto ret = transforms_cam.insert(std::pair<string, geometry_msgs::TransformStamped>(CAM_FRAMES[i], transform));
        if(!ret.second){
          ROS_ERROR("transform for frame %s already exists!", ret.first->first.c_str());
        }
      }
    } catch (tf2::TransformException &ex) {
        ROS_WARN("%s",ex.what());
        ros::Duration(1.0).sleep();
        continue;
    }

    if(transforms_cam.size() == NUM_CAMERAS){
      success = true;
      ROS_INFO("Sucessfully retrieved camera extrinsic transforms.");
    }
    else {
      ROS_ERROR("Wrong number of camera transforms! (need to get %d transforms, but received %zu.", NUM_CAMERAS, transforms_cam.size());
    }
  }
}

void cameraInfoCallback(const sensor_msgs::CameraInfo::ConstPtr msg, sensor_msgs::CameraInfo& msg_out){
  msg_out = *msg;
}

void getIntrinsics(vector<sensor_msgs::CameraInfo>& intrinsics, ros::NodeHandle& nh){
  vector<ros::Subscriber> intrinsic_subs;
  intrinsics.resize(NUM_CAMERAS);
  for (size_t i = 0; i < NUM_CAMERAS; ++i) {
    auto sub = nh.subscribe<sensor_msgs::CameraInfo>(CAM_INFO_TOPICS[i], 1, std::bind(cameraInfoCallback, std::placeholders::_1, std::ref(intrinsics[i])));
    intrinsic_subs.push_back(sub);
  }

  ros::Rate rate(1.0);
  bool success = false;
  while(!success && ros::ok()){
    ROS_INFO("Spinning.. Waiting to receive camera intrinsics.");
    ros::spinOnce();
    rate.sleep();

    success = true;
    for (const auto& intr : intrinsics) {
      if (intr.D.size() == 0 && intr.distortion_model != "none"){ //D empty as no distortion modeled
        success = false;
        break;
      }
    }
  }

  intrinsic_subs.clear();
  ROS_INFO("intrinsics received.");
  for (const auto& intr : intrinsics) {
    cout << "fx: " << intr.K[0] << ", fy: " << intr.K[4] << ", cx: " << intr.K[2] << ", cy: " << intr.K[5] << endl;
  }
}

void fusedSkeletonCallback(const PersonCovList::ConstPtr& persons_msg, map<string, geometry_msgs::TransformStamped>& transforms_cam, const vector<sensor_msgs::CameraInfo>& intrinsics, const vector<ros::Publisher>& skel_pubs){
  if(persons_msg->header.frame_id != BASE_FRAME){
    ROS_ERROR("Fused person is not given in \"%s\" but in \"%s\". Aborting!", BASE_FRAME.c_str(), persons_msg->header.frame_id.c_str());
    return;
  }

  const int num_trasforms = transforms_cam.size();
  const int num_persons = persons_msg->persons.size();
  vector<Person2DList> persons_proj_vec(num_trasforms);
  vector<Eigen::Affine3d> to_cam_eigen(num_trasforms);
  vector<image_geometry::PinholeCameraModel> cam_intrinsics(num_trasforms);
  std::vector<std::vector<double> > min_x(num_trasforms), min_y(num_trasforms), max_x(num_trasforms, std::vector<double>(num_persons, 0)), max_y(num_trasforms, std::vector<double>(num_persons, 0));

  for (int i = 0; i < num_trasforms; ++i) {
    string cam_frame = intrinsics[i].header.frame_id;
    to_cam_eigen[i] = tf2::transformToEigen(transforms_cam[cam_frame]);
    cam_intrinsics[i].fromCameraInfo(intrinsics[i]);

    persons_proj_vec[i].header.frame_id = intrinsics[i].header.frame_id;
    persons_proj_vec[i].header.stamp = persons_msg->ts_per_cam[i];
    persons_proj_vec[i].fb_delay = persons_msg->fb_delay_per_cam[i];

    min_x[i] = std::vector<double>(num_persons, intrinsics[i].width);
    min_y[i] = std::vector<double>(num_persons, intrinsics[i].height);
  }

  for (int person_idx = 0; person_idx < num_persons; ++person_idx) {
    if(persons_msg->persons[person_idx].keypoints.size() != FUSION_BODY_PARTS::NUM_KEYPOINTS){
      ROS_ERROR("Fused person %d: Expected skeleton to have %d Keypoints, but got %zu. Aborting!", person_idx, FUSION_BODY_PARTS::NUM_KEYPOINTS, persons_msg->persons[person_idx].keypoints.size());
      continue;
    }

    std::vector<Person2D> person_in_cam(num_trasforms);
    std::vector<int> num_valid_kps_in_cam(num_trasforms, 0);
    for (int i = 0; i < num_trasforms; ++i) {
      person_in_cam[i].keypoints.resize(NUM_KEYPOINTS);
      person_in_cam[i].score = 1.0; // TODO, no per-person total score available
    }

    for (int kp_idx = 0; kp_idx < NUM_KEYPOINTS; ++kp_idx) {
      const auto& kp_3d = persons_msg->persons[person_idx].keypoints.at(g_kp2kpFusion_idx[kp_idx]);

      if(kp_3d.score <= 0.0f) //if(kp_3d.joint.x == 0.0 && kp_3d.joint.y == 0.0 && kp_3d.joint.z == 0.0)
        continue;

      Eigen::Matrix3d cov;
      cov << kp_3d.cov[0], kp_3d.cov[1], kp_3d.cov[2],
             kp_3d.cov[1], kp_3d.cov[3], kp_3d.cov[4],
             kp_3d.cov[2], kp_3d.cov[4], kp_3d.cov[5];
      Eigen::Vector3d joint_base(kp_3d.joint.x, kp_3d.joint.y, kp_3d.joint.z);
      SamplesMatType samples;
      SamplesWeightType weights = draw_sigma_points(samples, joint_base, cov); //draw sigma points.
      TransformedSampledMatType weights_replicated = weights.replicate<2,1>();

      for (int i = 0; i < num_trasforms; ++i) {
        TransformedSampledMatType samples_proj;
        for(int sample_idx = 0; sample_idx < N_SAMPLES; ++sample_idx){ //project sigma-points to image plane
          Eigen::Vector3d joint_cam = to_cam_eigen[i] * samples.col(sample_idx); // project joint to camera frame
          cv::Point2d joint_px = cam_intrinsics[i].project3dToPixel(cv::Point3d(joint_cam.x(), joint_cam.y(), joint_cam.z())); // project joint to image plane
          samples_proj(0, sample_idx) = joint_px.x;
          samples_proj(1, sample_idx) = joint_px.y;
        }

        Eigen::Vector2d joint_px = (samples_proj.cwiseProduct(weights_replicated)).rowwise().sum();
        TransformedSampledMatType samples_proj_centered = samples_proj - joint_px.replicate<1, N_SAMPLES>();
        Eigen::Matrix2d cov_2d = samples_proj_centered.cwiseProduct(weights_replicated) * samples_proj_centered.transpose();
        //cout << "Joint: " << kp_idx << ", camera: " << person_idx << ": pixel-covariance: " << endl << cov_2d << endl;

        if(joint_px.x() < 0 || joint_px.x() > intrinsics[i].width || joint_px.y() < 0 || joint_px.y() > intrinsics[i].height) // joint is outside of image plane
          continue;

        ++num_valid_kps_in_cam[i];
        person_in_cam[i].keypoints[kp_idx].x = static_cast<float>(joint_px.x()); // abs coordinates
        person_in_cam[i].keypoints[kp_idx].y = static_cast<float>(joint_px.y()); // abs coordinates
        person_in_cam[i].keypoints[kp_idx].score = kp_3d.score;
        person_in_cam[i].keypoints[kp_idx].cov[0] = static_cast<float>(cov_2d(0, 0));
        person_in_cam[i].keypoints[kp_idx].cov[1] = static_cast<float>(cov_2d(0, 1));
        person_in_cam[i].keypoints[kp_idx].cov[2] = static_cast<float>(cov_2d(1, 1));

        if (joint_px.x() < min_x[i][person_idx]){min_x[i][person_idx] = joint_px.x();}
        if (joint_px.y() < min_y[i][person_idx]){min_y[i][person_idx] = joint_px.y();}
        if (joint_px.x() > max_x[i][person_idx]){max_x[i][person_idx] = joint_px.x();}
        if (joint_px.y() > max_y[i][person_idx]){max_y[i][person_idx] = joint_px.y();}
      }
    }

    for (int i = 0; i < num_trasforms; ++i) {
      if(num_valid_kps_in_cam[i] > 0){
        person_in_cam[i].bbox = {(float)min_x[i][person_idx], (float)min_y[i][person_idx], (float)max_x[i][person_idx], (float)max_y[i][person_idx]};
        persons_proj_vec[i].persons.push_back(person_in_cam[i]);
      }
    }
  }

  for (int i = 0; i < num_trasforms; ++i)
    skel_pubs[i].publish(persons_proj_vec[i]);
}

int main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "multi_skeleton_reprojection");
  ros::NodeHandle nh;
  ros::NodeHandle nh_private("~");

  nh_private.param<string>("pose_method", g_param_pose_method, "simple");
  std::vector<string> cam_names;
  nh_private.param("cameras", cam_names, std::vector<string>());
  if(cam_names.size() > 0){
    NUM_CAMERAS = cam_names.size();
    CAM_FRAMES.clear();
    CAM_INFO_TOPICS.clear();
    SKELETON_PRED_TOPICS.clear();
    for (int i = 0; i < NUM_CAMERAS; ++i) {
      CAM_FRAMES.push_back((string)cam_names[i] + g_cam_frame);
      CAM_INFO_TOPICS.push_back((string)cam_names[i] + g_cam_info_topic);
      SKELETON_PRED_TOPICS.push_back((string)cam_names[i] + g_skel_pred_topic);
    }
  }

  ROS_INFO("NUM_CAMERAS: %d, topics and frames: ", NUM_CAMERAS);
  for (int i = 0; i < NUM_CAMERAS; ++i) {
    cout << "\t" << CAM_FRAMES[i] << ", " << CAM_INFO_TOPICS[i] << ", " << SKELETON_PRED_TOPICS[i] << endl;
  }

  if (g_param_pose_method == "h36m")
    g_kp2kpFusion_idx = g_kp2kpFusion_idx_h36m;
  else
    g_kp2kpFusion_idx = g_kp2kpFusion_idx_simple;

  NUM_KEYPOINTS = 17;
  ROS_INFO("Using pose estimation method: %s, and %d keypoints.", g_param_pose_method.c_str(), NUM_KEYPOINTS);

  tf2_ros::Buffer tfBuffer;
  tf2_ros::TransformListener tfListener(tfBuffer);

  map<string, geometry_msgs::TransformStamped> transforms_cam;
  getTransforms(transforms_cam, tfBuffer);

  vector<sensor_msgs::CameraInfo> intrinsics;
  getIntrinsics(intrinsics, nh);

  vector<ros::Publisher> skeleton_pubs;
  for (int i = 0; i < NUM_CAMERAS; ++i) {
    skeleton_pubs.push_back(nh.advertise<Person2DList>(SKELETON_PRED_TOPICS[i], 1));
  }

  ROS_INFO("Reprojecting into %zu / %d camera views", transforms_cam.size(), NUM_CAMERAS);

  if(transforms_cam.size() != intrinsics.size() || transforms_cam.size() != skeleton_pubs.size() || intrinsics.size() != skeleton_pubs.size()){
    ROS_ERROR("incoherent number of transforms, intrinsics and output heatmaps! Aborting!");
    return -1;
  }

  ros::Subscriber fused_skel_sub = nh.subscribe<PersonCovList>(FUSED_SKELETON_TOPIC, 1, std::bind(fusedSkeletonCallback, std::placeholders::_1, std::ref(transforms_cam), std::cref(intrinsics), std::cref(skeleton_pubs)), ros::VoidConstPtr(), ros::TransportHints().tcpNoDelay());

  ros::spin();
}


#include "DataFormats/PatCandidates/interface/CovarianceParameterization.h"
#include "DataFormats/PatCandidates/interface/liblogintpack.h"
#include "DataFormats/PatCandidates/interface/libminifloat.h"
#include "FWCore/ParameterSet/interface/FileInPath.h"
#include <boost/format.hpp>
#include <iostream>
#include <TParameter.h>
#include <TVector.h>
#include <TFolder.h>

uint16_t CompressionElement::pack(float value, float ref) const
{
    float toCompress=0;
    switch(target) {
        case(realValue):
          toCompress=value;
          break;
        case(ratioToRef):
          toCompress=value/ref;
          break;
        case(differenceToRef):
          toCompress=value-ref;
          break;
    }
    switch(method) {
        case(float16):
          return MiniFloatConverter::float32to16(toCompress*params[0]);
          break;
        case(reduceMantissa):
          return MiniFloatConverter::reduceMantissaToNbits(toCompress,params[0]);
          break;
        case(zero):
          return 0;
          break;
        case(one):
          return 1.0;
          break;
        case(tanLogPack):
          return 0; //FIXME: should be implemented
          break;
        case(logPack):
          int16_t r=logintpack::pack16log(toCompress,params[0],params[1],bits);
          return * reinterpret_cast<uint16_t *>(&r);
          break;

    }
  return 0;
}
float CompressionElement::unpack(uint16_t packed, float ref) const
{
    float unpacked=0;
    switch(method) {
        case(float16):
          unpacked= MiniFloatConverter::float16to32(packed)/params[0];
          break;
        case(reduceMantissa):
          unpacked=packed;
          break;
        case(logPack):
          unpacked=logintpack::unpack16log(* reinterpret_cast<int16_t *>(&packed),params[0],params[1],bits);
          break;
        case(zero):
          unpacked=0;
          break;
        case(one):
        case(tanLogPack):
          unpacked=1; //FIXME: should be implemented
    }
    switch(target) {
        case(realValue):
          return unpacked;
        case(ratioToRef):
          return unpacked*ref;
        case(differenceToRef):
          return unpacked+ref;
    }

    return ref;

}

void CovarianceParameterization::load(int version)
{
 edm::FileInPath fip((boost::format("DataFormats/PatCandidates/data/CovarianceParameterization_version%d.root") % version).str());
 fileToRead_ = TFile::Open(fip.fullPath().c_str());
 TFile & fileToRead=*fileToRead_;
//Read files from here fip.fullPath().c_str();
 if(fileToRead.IsOpen())  {
     readFile(fileToRead);

     TIter next(((TDirectoryFile*)fileToRead.Get("schemas"))->GetListOfKeys());
     TKey *key;
     while ((key = (TKey*)next())) {
      TClass *cl = gROOT->GetClass(key->GetClassName());
      if (!cl->InheritsFrom("TDirectoryFile")) continue;
      std::string schemaNumber = key->ReadObj()->GetName();
      uint16_t schemaN = std::stoi(schemaNumber);
     //for (int folderNumber = 0; folderNumber < 6 ; folderNumber++) {
      CompressionSchema schema;
      for (int i = 0; i < 5; i++) {
            for (int j = i; j < 5; j++) {        //FILLING ONLY THE SCHEMA OF SOME ELEMENTS
                  std::string folder = "schemas/" + schemaNumber + "/"  + char(48+i) + char(48+j);
                std::string methodString = folder + "/method";
                std::string targetString = folder + "/target";
                std::string bitString = folder + "/bit";
                std::vector<float> vParams ;
                TVector *p=(TVector*) fileToRead.Get((folder+"/param").c_str());
                for(int k = 0 ; k < p->GetNoElements() ; k++){
                    vParams.push_back((*p)[k]);
                }

                schema(i,j)=CompressionElement((CompressionElement::Method) ((TParameter<int>*) fileToRead.Get(methodString.c_str()))->GetVal(),
		(CompressionElement::Target) ((TParameter<int>*) fileToRead.Get(targetString.c_str()))->GetVal(),
		 (int) ((TParameter<int>*) fileToRead.Get(bitString.c_str()))->GetVal(), vParams);

            }
        }
     schemas[schemaN]=schema;
     }

    loadedVersion_=version;
 } else {loadedVersion_=-1;}

}

void CovarianceParameterization::readFile( TFile & f) {

    for (int i = 0; i < 5; i++) {
        for (int j = i; j < 5; j++) {

            std::string String_first_positive = "_pixel_";
            std::string String_second_positive = "_noPixel_";

            addTheHistogram(&cov_elements_pixelHit, String_first_positive, i, j,f);
            addTheHistogram(&cov_elements_noPixelHit, String_second_positive, i, j,f);

        }
    }
}

void CovarianceParameterization::addTheHistogram(std::vector<TH3D *> * HistoVector, std::string StringToAddInTheName, int i, int j,  TFile & fileToRead) {

    std::string  List_covName[5] = {"qoverp", "lambda", "phi", "dxy", "dsz"};

    std::string histoNameString = "covariance_" + List_covName[i] + "_" + List_covName[j] + StringToAddInTheName+"parametrization" ;// + "_entries";
    TH3D * matrixElememtHistogramm = (TH3D*) fileToRead.Get(histoNameString.c_str());
    HistoVector->push_back(matrixElememtHistogramm);
}

float CovarianceParameterization::meanValue(int i,int j,int sign,float pt, float eta, int nHits,int pixelHits,  float cii,float cjj) const {
    int hitNumberToUse = nHits;
    if (hitNumberToUse < 2 ) hitNumberToUse = 2;
    if (hitNumberToUse > 32 ) hitNumberToUse = 32;
    int  ptBin = cov_elements_pixelHit[0]->GetXaxis()->FindBin(pt);
    int etaBin = cov_elements_pixelHit[0]->GetYaxis()->FindBin(std::abs(eta));
    int hitBin = cov_elements_pixelHit[0]->GetZaxis()->FindBin(hitNumberToUse);
    int min_idx = i;
    int max_idx = j;

    if (i>j) {
        min_idx = j;
        max_idx = i;
    }

    int indexOfTheHitogramInTheList = ((9 - min_idx)*min_idx)/2 + max_idx;

    double meanValue = 0.;
    if (pixelHits > 0) {
            meanValue =sign* cov_elements_pixelHit[indexOfTheHitogramInTheList]->GetBinContent(ptBin, etaBin, hitBin);
        }
        else {
            meanValue = sign*cov_elements_noPixelHit[indexOfTheHitogramInTheList]->GetBinContent(ptBin, etaBin, hitBin);
        }
    return meanValue;

}

float CovarianceParameterization::pack(float value, int schema, int i,int j,float pt, float eta, int nHits,int pixelHits,  float cii,float cjj) const {
    if(i>j) std::swap(i,j);
    float ref=meanValue(i,j,1.,pt,eta,nHits,pixelHits,cii,cjj);
    if(ref==0) {
      schema=0;
    }
    if(schema==0 && i==j && (i==2 || i==0) ) ref=1./(pt*pt);
/*  //Used for debugging, to be later removed
    uint16_t p=(*schemas.find(schema)).second(i,j).pack(value,ref);
    float up=(*schemas.find(schema)).second(i,j).unpack(p,ref);
    std::cout << "check " << i << " " << j << " " << value << " " << up << " " << p << " " << ref << " " << schema<< std::endl;*/
    return (*schemas.find(schema)).second(i,j).pack(value,ref);
}
float CovarianceParameterization::unpack(uint16_t packed, int schema, int i,int j,float pt, float eta, int nHits,int pixelHits,  float cii,float cjj) const {
    if(i>j) std::swap(i,j);
    float ref=meanValue(i,j,1.,pt,eta,nHits,pixelHits,cii,cjj);
    if(ref==0) {
      schema=0;
    }
    if(schema==0 && i==j && (i==2 || i==0) ) ref=1./(pt*pt);
    if(i==j && (*schemas.find(schema)).second(i,j).unpack(packed,ref)==0) return 1e-9;
    else
    return (*schemas.find(schema)).second(i,j).unpack(packed,ref);

}


// graph-tool -- a general graph modification and manipulation thingy
//
// Copyright (C) 2006-2015 Tiago de Paula Peixoto <tiago@skewed.de>
//
// This program is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public License
// as published by the Free Software Foundation; either version 3
// of the License, or (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.

#include "graph.hh"
#include "graph_filtering.hh"
#include "graph_properties.hh"
#include "graph_selectors.hh"

#include <boost/python.hpp>

#include <boost/graph/johnson_all_pairs_shortest.hpp>
#include <boost/graph/floyd_warshall_shortest.hpp>

using namespace std;
using namespace boost;
using namespace graph_tool;

struct do_all_pairs_search
{
    template <class Graph, class VertexIndexMap, class DistMap, class WeightMap>
    void operator()(const Graph& g, VertexIndexMap vertex_index,
                    DistMap dist_map, WeightMap weight, bool dense) const
    {
        typedef typename property_traits<DistMap>::value_type::value_type
            dist_t;

        int i, N = num_vertices(g);
        #pragma omp parallel for default(shared) private(i) schedule(runtime) if (N > 100)
        for (i = 0; i < N; ++i)
        {
            dist_map[i].clear();
            dist_map[i].resize(num_vertices(g), 0);
        }

        if (dense)
        {
            floyd_warshall_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
        else
        {
            johnson_all_pairs_shortest_paths
                (g, dist_map,
                 weight_map(ConvertedPropertyMap<WeightMap,dist_t>(weight)).
                 vertex_index_map(vertex_index));
        }
    }
};

void get_all_dists(GraphInterface& gi, boost::any dist_map, boost::any weight,
                   bool dense)
{
    typedef ConstantPropertyMap<size_t,GraphInterface::edge_t> cweight_map_t;

    if (weight.empty())
        weight = boost::any(cweight_map_t(1));

    run_action<>()
        (gi, std::bind(do_all_pairs_search(), placeholders::_1,
                       gi.GetVertexIndex(), placeholders::_2, placeholders::_3,
                       dense),
         vertex_scalar_vector_properties(),
         mpl::push_back<edge_scalar_properties,cweight_map_t>::type())
        (dist_map, weight);
}

void export_all_dists()
{
    python::def("get_all_dists", &get_all_dists);
};


#include "GapsRunner.h"
#include "GapsResult.h"
#include "GapsParameters.h"
#include "GapsStatistics.h"
#include "math/Random.h"
#include "utils/Archive.h"
#include "utils/GlobalConfig.h"
#include "gibbs_sampler/AsynchronousGibbsSampler.h"
#include "gibbs_sampler/SingleThreadedGibbsSampler.h"
#include "gibbs_sampler/DenseNormalModel.h"
#include "gibbs_sampler/SparseNormalModel.h"

#ifdef __GAPS_R_BUILD__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#include <Rcpp.h>
#pragma GCC diagnostic pop
#endif

#ifdef __GAPS_OPENMP__
#include <omp.h>
#endif

// boost time helpers
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wdeprecated-declarations"
#include <boost/date_time/posix_time/posix_time.hpp>
#pragma GCC diagnostic pop
namespace bpt = boost::posix_time;
#define bpt_now() bpt::microsec_clock::local_time()

// for conditionally printing status messages
#define GAPS_MESSAGE(b, m) \
    do { \
        if (b) { \
            gaps_printf(m); \
        } \
    } while(0)

// for converting seconds to h:m:s
struct GapsTime
{
    unsigned hours;
    unsigned minutes;
    unsigned seconds;
    explicit GapsTime(unsigned totalSeconds)
    {
        hours = totalSeconds / 3600;
        totalSeconds -= hours * 3600;
        minutes = totalSeconds / 60;
        totalSeconds -= minutes * 60;
        seconds = totalSeconds;
    }
};

// forward declaration
template <class Sampler, class DataType>
static GapsResult runCoGAPSAlgorithm(const DataType &data, GapsParameters &params,
    const DataType &uncertainty, GapsRandomState *randState);

////////////////////////////////////////////////////////////////////////////////

template <class DataModel, class DataType>
static GapsResult chooseSampler(const DataType &data, GapsParameters &params,
const DataType &uncertainty, GapsRandomState *randState)
{
    if (params.asynchronousUpdates)
    {
        GAPS_MESSAGE(params.printMessages, "Sampler Type: Asynchronous\n");
        return runCoGAPSAlgorithm< AsynchronousGibbsSampler<DataModel> >(data,
            params, uncertainty, randState);
    }
    GAPS_MESSAGE(params.printMessages, "Sampler Type: Sequential\n");
    return runCoGAPSAlgorithm< SingleThreadedGibbsSampler<DataModel> >(data,
        params, uncertainty, randState);
}

template <class DataType>
static GapsResult chooseDataModel(const DataType &data, GapsParameters &params,
const DataType &uncertainty, GapsRandomState *randState)
{
    if (params.useSparseOptimization)
    {
        GAPS_MESSAGE(params.printMessages, "Data Model: Sparse, Normal\n");
        return chooseSampler<SparseNormalModel>(data, params, uncertainty, randState);
    }
    GAPS_MESSAGE(params.printMessages, "Data Model: Dense, Normal\n");
    return chooseSampler<DenseNormalModel>(data, params, uncertainty, randState);
}

// helper function, this dispatches the correct run function depending
// on the type of GibbsSampler and DataModel needed for the given parameters
template <class DataType>
static GapsResult run_helper(const DataType &data, GapsParameters &params,
const DataType &uncertainty, GapsRandomState *randState)
{
    // fetch parameters from checkpoint - some are used in initialization
    if (params.useCheckPoint)
    {
        Archive ar(params.checkpointFile, ARCHIVE_READ);
        ar >> params;
        ar >> *randState;
    }
    return chooseDataModel(data, params, uncertainty, randState);
}

// these two functions are the top-level functions exposed to the C++
// code that is being wrapped by any given language

GapsResult gaps::run(const Matrix &data, GapsParameters &params,
const Matrix &uncertainty, GapsRandomState *randState)
{
    return run_helper(data, params, uncertainty, randState);
}

GapsResult gaps::run(const std::string &data, GapsParameters &params,
const std::string &uncertainty, GapsRandomState *randState)
{
    return run_helper(data, params, uncertainty, randState);
}

////////////////////////////////////////////////////////////////////////////////

// sum coef * log(i) for i = 1 to total, fit coef from number of atoms
// approximates sum of number of atoms (stirling approx to factorial)
// this should be proportional to total number of updates
static double estimatedNumUpdates(double current, double total, float nAtoms)
{
    double coef = nAtoms / std::log(current);
    return coef * std::log(std::sqrt(2 * total * gaps::pi)) +
        total * coef * std::log(total) - total * coef;
}

template <class Sampler>
static double estimatedPercentComplete(const GapsParameters &params,
const Sampler &ASampler, const Sampler &PSampler, GapsAlgorithmPhase phase, unsigned iter)
{
    double nIter = static_cast<double>(iter);
    double nAtomsA = static_cast<double>(ASampler.nAtoms());
    double nAtomsP = static_cast<double>(PSampler.nAtoms());

    if (phase == GAPS_SAMPLING_PHASE)
    {
        nIter += params.nIterations;
    }

    double totalIter = 2.0 * static_cast<double>(params.nIterations);
    double estimatedCompleted = estimatedNumUpdates(nIter, nIter, nAtomsA) +
        estimatedNumUpdates(nIter, nIter, nAtomsP);
    double estimatedTotal = estimatedNumUpdates(nIter, totalIter, nAtomsA) +
        estimatedNumUpdates(nIter, totalIter, nAtomsP);
    return estimatedCompleted / estimatedTotal;
}

template <class Sampler>
static void displayStatus(const GapsParameters &params,
const Sampler &ASampler, const Sampler &PSampler, bpt::ptime startTime,
GapsAlgorithmPhase phase, unsigned iter, GapsStatistics &stats)
{
    if (params.outputFrequency > 0 && ((iter + 1) % params.outputFrequency) == 0)
    {
        float cs = PSampler.chiSq();
        unsigned nA = ASampler.nAtoms();
        unsigned nP = PSampler.nAtoms();
        stats.addChiSq(cs);
        stats.addAtomCount(nA, nP);
        if (params.printMessages)
        {
            bpt::time_duration diff = bpt_now() - startTime;
            double perComplete = estimatedPercentComplete(params, ASampler,
                PSampler, phase, iter);

            GapsTime elapsedTime(static_cast<unsigned>(diff.total_seconds()));
            GapsTime totalTime(static_cast<unsigned>(diff.total_seconds() / perComplete));

            gaps_printf("%d of %d, Atoms: %d(A), %d(P), ChiSq: %.0f, Time: %02d:%02d:%02d / %02d:%02d:%02d\n",
                iter + 1, params.nIterations, nA, nP, cs, elapsedTime.hours,
                elapsedTime.minutes, elapsedTime.seconds, totalTime.hours,
                totalTime.minutes, totalTime.seconds);
            gaps_flush();
        }
    }
}

template <class Sampler>
static void updateSampler(const GapsParameters &params, Sampler &ASampler,
Sampler &PSampler, unsigned nA, unsigned nP)
{
    if (params.whichMatrixFixed != 'A')
    {
        ASampler.update(nA, params.maxThreads);
        if (params.whichMatrixFixed != 'P')
        {
            PSampler.sync(ASampler, params.maxThreads);
        }
    }

    if (params.whichMatrixFixed != 'P')
    {
        PSampler.update(nP, params.maxThreads);
        if (params.whichMatrixFixed != 'A')
        {
            ASampler.sync(PSampler, params.maxThreads);
        }
    }
}

template <class Sampler>
static void createCheckpoint(const GapsParameters &params,
Sampler &ASampler, Sampler &PSampler, const GapsRandomState *randState,
const GapsStatistics &stats, const GapsRng &rng, GapsAlgorithmPhase phase, unsigned iter)
{
    if (params.checkpointInterval > 0 && ((iter + 1) % params.checkpointInterval) == 0
    && !params.subsetData)
    {
        // create backup file
        std::rename(params.checkpointOutFile.c_str(),
            (params.checkpointOutFile + ".backup").c_str());

        // create checkpoint file
        Archive ar(params.checkpointOutFile, ARCHIVE_WRITE);
        ar << params;
        ar << *randState;
        ar << ASampler << PSampler << stats << static_cast<int>(phase) << iter << rng;

        // delete backup file
        std::remove((params.checkpointOutFile + ".backup").c_str());

        // running the extra initialization here allows for consistency with runs
        // started from a checkpoint. This initialization phase will be run first
        // thing once a checkpoint is loaded since large matrices which aren't stored
        // need to be initialized. By running it here we make sure that the algorithm
        // is in the same state it will be when started from a checkpoint
        ASampler.extraInitialization();
        PSampler.extraInitialization();
    }
}

template <class Sampler>
static void processCheckpoint(GapsParameters &params, Sampler &ASampler,
Sampler &PSampler, GapsRandomState *randState, GapsStatistics &stats,
GapsRng &rng, GapsAlgorithmPhase &phase, unsigned &currentIter)
{
    // check if running from checkpoint, get all saved data
    if (params.useCheckPoint)
    {
        int iPhase;
        Archive ar(params.checkpointFile, ARCHIVE_READ);
        ar >> params;
        ar >> *randState;
        ar >> ASampler >> PSampler >> stats >> iPhase >> currentIter >> rng;
        phase = static_cast<GapsAlgorithmPhase>(iPhase);
    }
}

template <class Sampler>
static uint64_t runOnePhase(const GapsParameters &params, Sampler &ASampler,
Sampler &PSampler, GapsStatistics &stats, const GapsRandomState *randState,
GapsRng &rng, bpt::ptime startTime, GapsAlgorithmPhase phase, unsigned &currentIter)
{
    uint64_t totalUpdates = 0;
    for (; currentIter < params.nIterations; ++currentIter)
    {
        gaps_check_interrupt();
        createCheckpoint(params, ASampler, PSampler, randState, stats,
            rng, phase, currentIter);

        // set annealing temperature in Equilibration phase
        if (phase == GAPS_EQUILIBRATION_PHASE)
        {
            float temp = static_cast<float>(2 * currentIter)
                / static_cast<float>(params.nIterations);
            ASampler.setAnnealingTemp(gaps::min(1.f, temp));
            PSampler.setAnnealingTemp(gaps::min(1.f, temp));
        }

        // number of updates per iteration is poisson
        unsigned nA = rng.poisson(gaps::max(ASampler.nAtoms(), 10));
        unsigned nP = rng.poisson(gaps::max(PSampler.nAtoms(), 10));
        updateSampler(params, ASampler, PSampler, nA, nP);
        totalUpdates += nA + nP;

        if (phase == GAPS_SAMPLING_PHASE)
        {
            stats.update(ASampler, PSampler);
            if (params.takePumpSamples)
            {
                stats.updatePump(ASampler);
            }
        }
        if (params.snapshotPhase == phase || params.snapshotPhase == GAPS_ALL_PHASES)
        {
            if (params.snapshotFrequency > 0 && ((currentIter + 1) % params.snapshotFrequency) == 0)
            {
                stats.takeSnapshot(phase, ASampler, PSampler);
            }
        }
        displayStatus(params, ASampler, PSampler, startTime, phase,
            currentIter, stats);
    }
    return totalUpdates;
}

template <class Sampler>
static void processFixedMatrix(const GapsParameters &params, Sampler &ASampler,
Sampler &PSampler)
{
    // check if we're fixing a matrix
    if (params.useFixedPatterns)
    {
        GAPS_ASSERT(params.fixedPatterns.nCol() == params.nPatterns);
        switch (params.whichMatrixFixed)
        {
            case 'A' :
                GAPS_ASSERT(params.fixedPatterns.nRow() == params.nGenes);
                ASampler.setMatrix(params.fixedPatterns);
                break;
            case 'P' :
                GAPS_ASSERT(params.fixedPatterns.nRow() == params.nSamples);
                PSampler.setMatrix(params.fixedPatterns);
                break;
            default: break; // 'N' for none
        }
    }
}

static void calculateNumberOfThreads(GapsParameters params)
{
    // calculate appropiate number of threads if compiled with openmp
    #ifdef __GAPS_OPENMP__
    if (params.printMessages && params.printThreadUsage)
    {
        unsigned availableThreads = omp_get_max_threads();
        params.maxThreads = gaps::min(availableThreads, params.maxThreads);
        gaps_printf("Running on %d out of %d available threads\n",
            params.maxThreads, availableThreads);
    }
    #endif
}

template <class Sampler, class DataType>
static void processUncertainty(const GapsParameters params, Sampler &ASampler,
Sampler &PSampler, const DataType &uncertainty)
{
    // read in the uncertainty matrix if one is provided
    if (!uncertainty.empty())
    {
        ASampler.setUncertainty(uncertainty, !params.transposeData,
            !params.subsetGenes, params);
        PSampler.setUncertainty(uncertainty, params.transposeData,
            params.subsetGenes, params);
    }
}

// here is the CoGAPS algorithm
template <class Sampler, class DataType>
static GapsResult runCoGAPSAlgorithm(const DataType &data, GapsParameters &params,
const DataType &uncertainty, GapsRandomState *randState)
{
    // check if running in debug mode
    #ifdef GAPS_DEBUG
    gaps_printf("Running in debug mode\n");
    #endif

    // load data into gibbs samplers
    // we transpose the data in the A sampler so that the update step
    // is symmetrical for each sampler, this simplifies the code
    // within the sampler, note the subsetting genes/samples flag must be
    // flipped if we are flipping the transpose flag
    // note: there are excessive amounts of check interrupt statements here,
    // this is so that if the user accidentally runs cogaps on an extremely
    // large file they can exit during the loading phase rather than killing
    // the process or waiting for it to finish
    GAPS_MESSAGE(params.printMessages, "Loading Data...");
    bpt::ptime readStart = bpt_now();
    gaps_check_interrupt();
    Sampler ASampler(data, !params.transposeData, !params.subsetGenes,
        params.alphaA, params.maxGibbsMassA, params, randState);
    gaps_check_interrupt();
    Sampler PSampler(data, params.transposeData, params.subsetGenes,
        params.alphaP, params.maxGibbsMassP, params, randState);
    gaps_check_interrupt();
    processUncertainty(params, ASampler, PSampler, uncertainty);
    gaps_check_interrupt();
    processFixedMatrix(params, ASampler, PSampler);
    gaps_check_interrupt();

    // elapsed time for reading data
    bpt::time_duration readDiff = bpt_now() - readStart;
    GapsTime elapsed(static_cast<unsigned>(readDiff.total_seconds()));
    if (params.printMessages)
    {
        gaps_printf("Done! (%02d:%02d:%02d)\n", elapsed.hours, elapsed.minutes,
            elapsed.seconds);
    }

    // check if data is sparse and sparseOptimization is not enabled
    if (params.printMessages && !params.useSparseOptimization && ASampler.dataSparsity() > 0.80f)
    {
        gaps_printf("\nWarning: data is more than 80%% sparse and sparseOptimization is not enabled\n");
    }

    // if we are running distributed, each worker needs to print when it's started
    if (params.runningDistributed)
    {
        gaps_printf("    worker %d is starting!\n", params.workerID);
        gaps_flush();
    }

    // these variables will get overwritten by checkpoint if provided
    GapsStatistics stats(params.nGenes, params.nSamples, params.nPatterns);
    GapsRng rng(randState);
    GapsAlgorithmPhase phase(GAPS_EQUILIBRATION_PHASE);
    unsigned currentIter = 0;
    processCheckpoint(params, ASampler, PSampler, randState, stats, rng, phase, currentIter);
    calculateNumberOfThreads(params);

    // sync samplers and run any additional initialization needed
    ASampler.sync(PSampler);
    PSampler.sync(ASampler);
    ASampler.extraInitialization();
    PSampler.extraInitialization();

    // record start time
    bpt::ptime startTime = bpt_now();

    // fallthrough through phases, allows algorithm to be resumed in either phase
    GAPS_ASSERT(phase == GAPS_EQUILIBRATION_PHASE || phase == GAPS_SAMPLING_PHASE);
    uint64_t totalUpdates = 0;
    switch (phase)
    {
        case GAPS_EQUILIBRATION_PHASE:
            GAPS_MESSAGE(params.printMessages, "-- Equilibration Phase --\n");
            totalUpdates += runOnePhase(params, ASampler, PSampler, stats, randState,
                rng, startTime, phase, currentIter);
            phase = GAPS_SAMPLING_PHASE;
            currentIter = 0;
        // fall through
        case GAPS_SAMPLING_PHASE:
            GAPS_MESSAGE(params.printMessages, "-- Sampling Phase --\n");
            totalUpdates += runOnePhase(params, ASampler, PSampler, stats, randState,
                rng, startTime, phase, currentIter);
    }

    // get result
    GapsResult result(stats);
    result.totalRunningTime = static_cast<unsigned>((bpt_now() - startTime).total_seconds());
    result.meanChiSq = stats.meanChiSq(PSampler);
    result.averageQueueLengthA = ASampler.getAverageQueueLength();
    result.averageQueueLengthP = PSampler.getAverageQueueLength();
    result.totalUpdates = totalUpdates;

    // handle pump statistics
    if (params.takePumpSamples)
    {
        result.pumpMatrix = stats.pumpMatrix();
        result.meanPatternAssignment = stats.meanPattern();
    }

    // if we are running distributed, each worker needs to print when it's done
    if (params.runningDistributed)
    {
        GapsTime elapsed(static_cast<unsigned>((bpt_now() - startTime).total_seconds()));
        gaps_printf("    worker %d is finished! Time: %02d:%02d:%02d\n",
            params.workerID, elapsed.hours, elapsed.minutes, elapsed.seconds);
        gaps_flush();
    }
    return result;
}


/*
@copyright Louis Dionne 2015
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_HANA_TEST_TEST_AUTO_GROUP_HPP
#define BOOST_HANA_TEST_TEST_AUTO_GROUP_HPP

#include <boost/hana/group.hpp>

#include <boost/hana/assert.hpp>
#include <boost/hana/comparable.hpp>
#include <boost/hana/core/models.hpp>
#include <boost/hana/monoid.hpp>

#include <test/auto/base.hpp>
#include <test/auto/monoid.hpp>

namespace boost { namespace hana { namespace test {
    template <typename G>
    auto laws<Group, G> = [] {
        static_assert(_models<Group, G>{}, "");

        laws<Monoid, G>();

        // Instance laws
        {
            for_each(objects<G>, [](auto x) {
                BOOST_HANA_CHECK(equal(
                    plus(x, negate(x)),
                    zero<G>()
                ));

                BOOST_HANA_CHECK(equal(
                    plus(negate(x), x),
                    zero<G>()
                ));
            });
        }

        // minus
        {
            for_each(objects<G>, [](auto x) {
                for_each(objects<G>, [=](auto y) {
                    BOOST_HANA_CHECK(equal(
                        minus(x, y),
                        plus(x, negate(y))
                    ));

                    BOOST_HANA_CHECK(equal(
                        minus(y, x),
                        plus(y, negate(x))
                    ));
                });
            });
        }

        // negate
        {
            for_each(objects<G>, [](auto x) {
                BOOST_HANA_CHECK(equal(
                    negate(negate(x)),
                    x
                ));
            });
        }
    };
}}} // end namespace boost::hana::test

#endif // !BOOST_HANA_TEST_TEST_AUTO_GROUP_HPP