//
// Project: evolution_simulator_2
// File: CreatureCognition.hpp
//
// Copyright (c) 2021 Miika 'Lehdari' Lehtimäki
// You may use, distribute and modify this code under the terms
// of the licence specified in file LICENSE which is distributed
// with this source code package.
//

#ifndef EVOLUTION_SIMULATOR_2_CREATURECOGNITION_HPP
#define EVOLUTION_SIMULATOR_2_CREATURECOGNITION_HPP

#include <Eigen/Dense>

class Genome;

class CreatureCognition {
public:
    static constexpr uint64_t   inputSize = 11;
    static constexpr uint64_t   outputSize = 3;
    static constexpr uint64_t   memorySize = 16;

    static constexpr uint64_t   attackInputSize = 6;
    static constexpr uint64_t   attackOutputSize = 1;

    static constexpr uint64_t   hidden1Size = 16;
    static constexpr uint64_t   hidden2Size = 32;
    static constexpr uint64_t   hidden3Size = 16;
    static constexpr uint64_t   hidden4Size = 8;

    static constexpr uint64_t   attackHidden1Size = 8;

    template <int InputSize, int OutputSize>
    using Layer = Eigen::Matrix<float, OutputSize, InputSize>;

    template<typename>
    struct Dims;

    template<int InputSize, int OutputSize>
    struct Dims<Layer<InputSize, OutputSize>> {
        static constexpr uint64_t   input = InputSize;
        static constexpr uint64_t   output = OutputSize;
        static constexpr uint64_t   total = InputSize*OutputSize;
    };

    using Layer1 = Layer<inputSize+1, hidden1Size>; // +1 for bias term
    using Layer2 = Layer<hidden1Size+memorySize+1, hidden2Size>;
    using Layer3 = Layer<hidden2Size+1, hidden3Size>;
    using Layer3MemoryValue = Layer<hidden2Size+1, memorySize>;
    using Layer3MemoryGate = Layer<hidden2Size+1, memorySize>;
    using Layer4 = Layer<hidden3Size+memorySize+1, hidden4Size>;
    using Layer5 = Layer<hidden4Size+1, outputSize>;
    using AttackLayer1 = Layer<attackInputSize+1, attackHidden1Size>;
    using AttackLayer2 = Layer<attackHidden1Size+memorySize+1, attackOutputSize>;

    using Input = Eigen::Matrix<float, inputSize+1, 1>;
    using Output = Eigen::Matrix<float, outputSize, 1>;
    using Memory = Eigen::Matrix<float, memorySize, 1>;
    using AttackInput = Eigen::Matrix<float, attackInputSize+1, 1>;
    using AttackOutput = Eigen::Matrix<float, attackOutputSize, 1>;

    CreatureCognition(const Genome& genome);

    const Output& forward();

    AttackOutput attack(AttackInput& input) const;

    const Memory& getMemory() const;

    static constexpr uint64_t totalSize =
        Dims<Layer1>::total +
        Dims<Layer2>::total +
        Dims<Layer3>::total +
        Dims<Layer3MemoryValue>::total +
        Dims<Layer3MemoryGate>::total +
        Dims<Layer4>::total +
        Dims<Layer5>::total +
        Dims<AttackLayer1>::total +
        Dims<AttackLayer2>::total;

    friend class CreatureSystem;

private:
    Layer1              _layer1 = Layer1::Zero();
    Layer2              _layer2 = Layer2::Zero();
    Layer3              _layer3 = Layer3::Zero();
    Layer3MemoryValue   _layer3MemoryValue = Layer3MemoryValue::Zero();
    Layer3MemoryGate    _layer3MemoryGate = Layer3MemoryGate::Zero();
    Layer4              _layer4 = Layer4::Zero();
    Layer5              _layer5 = Layer5::Zero();
    AttackLayer1        _attackLayer1 = AttackLayer1::Zero();
    AttackLayer2        _attackLayer2 = AttackLayer2::Zero();

    Input               _input = Input::Zero();
    Output              _output = Output::Zero();
    Memory              _memory = Memory::Zero();
};

#endif //EVOLUTION_SIMULATOR_2_CREATURECOGNITION_HPP


//***************************************************************************
//* Copyright (c) 2015 Saint Petersburg State University
//* Copyright (c) 2011-2014 Saint Petersburg Academic University
//* All Rights Reserved
//* See file LICENSE for details.
//***************************************************************************

#include "subcluster.hpp"
#include "config_struct.hpp"
#include "consensus.hpp"
#include "hkmer_distance.hpp"
#include "kmer_data.hpp"
#include "utils/logger/log_writers.hpp"

#include <boost/numeric/ublas/matrix.hpp>

#include <iostream>
#include <vector>
#include "quality_metrics.h"
#include <boost/math/special_functions/gamma.hpp>

using namespace hammer;
using namespace hammer_config;
using namespace n_gamma_poisson_model;

double TGenomicHKMersEstimator::GenerateLikelihood(const HKMer& from,
                                                   const HKMer& to) const {
  double llGenerate = 0;
  for (size_t i = 0; i < hammer::K; ++i) {
    llGenerate += cluster_model_.ErrorLogLikelihood(from[i].len, to[i].len);
  }
  return llGenerate;
}

HKMer TGenomicHKMersEstimator::Center(const KMerData& data,
                                      const std::vector<size_t>& kmers) {
  hammer::HKMer res;
  namespace numeric = boost::numeric::ublas;

  for (unsigned i = 0; i < hammer::K; ++i) {
    numeric::matrix<double> scores(4, 64, 0);
    for (size_t j = 0; j < kmers.size(); ++j) {
      const hammer::KMerStat& k = data[kmers[j]];
// FIXME: switch to MLE when we'll have use per-run quality values
#if 1
      scores(k.kmer[i].nucl, k.kmer[i].len) += k.count * (1.0 - exp(k.qual));
#else
      for (unsigned n = 0; n < 4; ++n)
        for (unsigned l = 1; l < 64; ++l)
          scores(n, l) += k.count * (n == k.kmer[i].nucl && l == k.kmer[i].len
                                         ? log(1 - k.qual)
                                         : log(k.qual) - log(4 * 63 - 1));
#endif
    }

    res[i] = hammer::iontorrent::consensus(scores).first;
  }

  return res;
}

HKMer TGenomicHKMersEstimator::ByPosteriorQualCenter(
    const std::vector<size_t>& kmers) {
  hammer::HKMer res;
  namespace numeric = boost::numeric::ublas;

  for (unsigned i = 0; i < hammer::K; ++i) {
    numeric::matrix<double> scores(4, 64, 0);
    for (size_t j = 0; j < kmers.size(); ++j) {
      const hammer::KMerStat& kmerStat = data_[kmers[j]];
      scores(kmerStat.kmer[i].nucl, kmerStat.kmer[i].len) +=
          kmerStat.count * exp(cluster_model_.GenomicLogLikelihood(kmerStat));
    }

    res[i] = hammer::iontorrent::consensus(scores).first;
  }

  return res;
}

void TGenomicHKMersEstimator::ProceedCluster(std::vector<size_t>& cluster) {
  std::sort(cluster.begin(), cluster.end(), CountCmp(data_));

  std::vector<double> qualities;
  std::vector<size_t> candidates;

  for (size_t i = 0; i < cluster.size(); ++i) {

    const auto idx = cluster[i];
    const auto& stat = data_[idx];

    if ((uint)stat.count < cfg::get().subcluster_min_count && (i > 0)) {
      break;
    }

    const double qual = cluster_model_.StatTransform(stat);
    const double posterior = cluster_model_.GenomicLogLikelihood(stat);

    if (!std::isfinite(posterior)) {
      continue;
    }

    if (posterior > cfg::get().subcluster_threshold || i == 0) {
      candidates.push_back(idx);
      qualities.push_back(qual);
    }
  }

  std::vector<double> distOneBestQualities(qualities);
  std::vector<double> countThreshold(qualities.size());

  std::vector<double> kmerErrorRates;

  {
    for (size_t i = 0; i < candidates.size(); ++i) {
      const auto& centerCandidate = data_[candidates[i]];
      kmerErrorRates.push_back(exp(GenerateLikelihood(centerCandidate.kmer, centerCandidate.kmer)));

      for (size_t j = 0; j < i; ++j) {
        const auto& parent = data_[candidates[j]];

        if (cfg::get().subcluster_filter_by_count_enabled) {
          const double mult = pow(cfg::get().subcluster_count_mult, hammer::hkmerDistance(parent.kmer, centerCandidate.kmer).levenshtein_);
          countThreshold[i] +=  mult * parent.count / kmerErrorRates[j];
        }

        if (hammer::hkmerDistance(parent.kmer, centerCandidate.kmer).levenshtein_ <= 1) {
          distOneBestQualities[i] = std::min(distOneBestQualities[i], qualities[j]);
        }
      }

      auto distOneParents = FindDistOneFullDels(centerCandidate);
      for (auto distOneParent : distOneParents) {
        const auto& parent = data_[distOneParent];
        distOneBestQualities[i] = std::min(distOneBestQualities[i], cluster_model_.StatTransform(parent));

        if (cfg::get().subcluster_filter_by_count_enabled) {
          countThreshold[i] += cfg::get().subcluster_count_mult * parent.count / 10 / exp(GenerateLikelihood(parent.kmer, parent.kmer));
        }
      }
    }
  }

  std::vector<size_t> centerCandidates;

  const double qualMult = cfg::get().subcluster_qual_mult;
  //  const double alpha = cfg::get().dist_one_subcluster_alpha;

  for (size_t i = 0; i < candidates.size(); ++i) {
    const auto& candidate = data_[candidates[i]];

    //don't subcluster low coverage hkmers with long runs, we can't distinguish dist-one error from noise.
    if (cfg::get().subcluster_filter_by_count_enabled) {
         double upperCountThreshold = boost::math::gamma_q_inva(countThreshold[i] + 1, 0.99) - 1;
         if (candidate.count <= upperCountThreshold) {
            continue;
        }
    }

    if (i != 0 && distOneBestQualities[i] * qualMult < qualities[i]) {
      if (!cluster_model_.IsHighQuality(candidate)) {
        continue;
      }
    }
    centerCandidates.push_back(candidates[i]);
  }

  if (!centerCandidates.size()) {
    return;
  }

  // First consensus (it's also filtering step)
  if (consensus_type_ != CenterType::COUNT_ARGMAX) {
    std::set<size_t> centerCandidatesSet;
    const size_t k = centerCandidates.size();
    // Find the closest center
    std::vector<HKMer> centralKmers;
    std::vector<std::vector<size_t> > subclusters(k, std::vector<size_t>());

    for (size_t i = 0; i < k; ++i) {
      auto centerId = centerCandidates[i];
      centralKmers.push_back(data_[centerId].kmer);
    }

    for (size_t i = 0; i < cluster.size(); ++i) {
      double dist = std::numeric_limits<double>::infinity();
      size_t cidx = k;
      size_t count = 0;

      size_t kmerIdx = cluster[i];
      hammer::HKMer kmerx = data_[kmerIdx].kmer;

      for (size_t j = 0; j < k; ++j) {
        hammer::HKMer kmery = centralKmers[j];
        double cdist = hammer::hkmerDistance(kmerx, kmery).levenshtein_;
        if (cdist < dist || (cdist == dist && count < (size_t)data_[kmery].count)) {
          cidx = j;
          dist = cdist;
          count = data_[kmery].count;
        }
      }
      VERIFY(cidx < k);
      subclusters[cidx].push_back(cluster[i]);
    }

    for (size_t i = 0; i < k; ++i) {
      const auto& subcluster = subclusters[i];

      HKMer center;
      if (consensus_type_ == CenterType::CONSENSUS) {
        center = Center(data_, subcluster);
      } else if (consensus_type_ == CenterType::BY_POSTERIOR_QUALITY) {
        center = ByPosteriorQualCenter(subcluster);
      } else {
        INFO("Unsupported center type: will use mean instead");
        center = Center(data_, subcluster);
      }

      auto centerIdx = data_.checking_seq_idx(center);

      if ((k == 1 && centerIdx != -1ULL) || (centerIdx == centerCandidates[i])) {
        centerCandidatesSet.insert(centerIdx);
      }
    }

    centerCandidates = std::vector<size_t>(centerCandidatesSet.begin(),
                                           centerCandidatesSet.end());
  }

  std::vector<double> posteriorQualities;
  // Now let's "estimate" quality
  std::vector<char> distOneGoodCenters(centerCandidates.size());

  for (uint k = 0; k < centerCandidates.size(); ++k) {
    const auto idx = centerCandidates[k];
    const KMerStat& centerCandidate = data_[idx];
    for (uint j = 0; j < centerCandidates.size(); ++j) {
      if (hammer::hkmerDistance(centerCandidate.kmer, data_[centerCandidates[j]].kmer).hamming_ == 1) {
        distOneGoodCenters[k] = 1;
      }
    }
    double quality = cluster_model_.GenomicLogLikelihood(centerCandidate);
    quality = std::isfinite(quality) ? quality : -1000;
    posteriorQualities.push_back(std::max(quality, -1000.0));
  }

  for (size_t i = 0; i < posteriorQualities.size(); ++i) {
    const auto idx = centerCandidates[i];
    data_[idx].lock();
    const bool wasGood = data_[idx].good();
    data_[idx].posterior_genomic_ll = (float)std::max(posteriorQualities[i], (double)data_[idx].posterior_genomic_ll);
    data_[idx].dist_one_subcluster |= distOneGoodCenters[i];
    data_[idx].unlock();
    if (!wasGood && data_[idx].good()) {
#pragma omp atomic
      GoodKmers++;
    }
    if (!wasGood && data_[idx].skip()) {
#pragma omp atomic
      SkipKmers++;
    }
    if (wasGood) {
#pragma omp atomic
      ReasignedByConsenus++;
    }
  }
}


/*
 [auto_generated]
 boost/numeric/odeint/stepper/base/symplectic_rkn_stepper_base.hpp

 [begin_description]
 Base class for symplectic Runge-Kutta-Nystrom steppers.
 [end_description]

 Copyright 2011-2013 Karsten Ahnert
 Copyright 2011-2013 Mario Mulansky
 Copyright 2012 Christoph Koke

 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_NUMERIC_ODEINT_STEPPER_BASE_SYMPLECTIC_RKN_STEPPER_BASE_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_STEPPER_BASE_SYMPLECTIC_RKN_STEPPER_BASE_HPP_INCLUDED

#include <boost/array.hpp>

#include <boost/numeric/odeint/util/bind.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>

#include <boost/numeric/odeint/util/copy.hpp>
#include <boost/numeric/odeint/util/is_pair.hpp>

#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>

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

#include <boost/numeric/odeint/stepper/base/algebra_stepper_base.hpp>

namespace boost {
namespace numeric {
namespace odeint {

template<
size_t NumOfStages ,
unsigned short Order ,
class Coor ,
class Momentum ,
class Value ,
class CoorDeriv ,
class MomentumDeriv ,
class Time ,
class Algebra ,
class Operations ,
class Resizer
>
class symplectic_nystroem_stepper_base : public algebra_stepper_base< Algebra , Operations >
{

public:

    typedef algebra_stepper_base< Algebra , Operations > algebra_stepper_base_type;
    typedef typename algebra_stepper_base_type::algebra_type algebra_type;
    typedef typename algebra_stepper_base_type::operations_type operations_type;

    const static size_t num_of_stages = NumOfStages;
    typedef Coor coor_type;
    typedef Momentum momentum_type;
    typedef std::pair< coor_type , momentum_type > state_type;
    typedef CoorDeriv coor_deriv_type;
    typedef state_wrapper< coor_deriv_type> wrapped_coor_deriv_type;
    typedef MomentumDeriv momentum_deriv_type;
    typedef state_wrapper< momentum_deriv_type > wrapped_momentum_deriv_type;
    typedef std::pair< coor_deriv_type , momentum_deriv_type > deriv_type;
    typedef Value value_type;
    typedef Time time_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;

    #ifndef DOXYGEN_SKIP
    typedef symplectic_nystroem_stepper_base< NumOfStages , Order , Coor , Momentum , Value ,
            CoorDeriv , MomentumDeriv , Time , Algebra , Operations , Resizer > internal_stepper_base_type;
    #endif
    typedef unsigned short order_type;

    static const order_type order_value = Order;

    typedef boost::array< value_type , num_of_stages > coef_type;

    symplectic_nystroem_stepper_base( const coef_type &coef_a , const coef_type &coef_b , const algebra_type &algebra = algebra_type() )
        : algebra_stepper_base_type( algebra ) , m_coef_a( coef_a ) , m_coef_b( coef_b ) ,
          m_dqdt_resizer() , m_dpdt_resizer() , m_dqdt() , m_dpdt()
    { }

    order_type order( void ) const
    {
        return order_value;
    }

    /*
     * Version 1 : do_step( system , x , t , dt )
     *
     * This version does not solve the forwarding problem, boost.range can not be used.
     */
    template< class System , class StateInOut >
    void do_step( System system , const StateInOut &state , time_type t , time_type dt )
    {
        typedef typename odeint::unwrap_reference< System >::type system_type;
        do_step_impl( system , state , t , state , dt , typename is_pair< system_type >::type() );
    }

    /**
     * \brief Same function as above. It differs only in a different const specifier in order
     * to solve the forwarding problem, can be used with Boost.Range.
     */
    template< class System , class StateInOut >
    void do_step( System system , StateInOut &state , time_type t , time_type dt )
    {
        typedef typename odeint::unwrap_reference< System >::type system_type;
        do_step_impl( system , state , t , state , dt , typename is_pair< system_type >::type() );
    }

    /*
     * Version 2 : do_step( system , q , p , t , dt );
     *
     * For Convenience
     *
     * The two overloads are needed in order to solve the forwarding problem.
     */
    template< class System , class CoorInOut , class MomentumInOut >
    void do_step( System system , CoorInOut &q , MomentumInOut &p , time_type t , time_type dt )
    {
        do_step( system , std::make_pair( detail::ref( q ) , detail::ref( p ) ) , t , dt );
    }

    /**
     * \brief Same function as do_step( system , q , p , t , dt ). It differs only in a different const specifier in order
     * to solve the forwarding problem, can be called with Boost.Range.
     */
    template< class System , class CoorInOut , class MomentumInOut >
    void do_step( System system , const CoorInOut &q , const MomentumInOut &p , time_type t , time_type dt )
    {
        do_step( system , std::make_pair( detail::ref( q ) , detail::ref( p ) ) , t , dt );
    }

    /*
     * Version 3 : do_step( system , in , t , out , dt )
     *
     * The forwarding problem is not solved in this version
     */
    template< class System , class StateIn , class StateOut >
    void do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
    {
        typedef typename odeint::unwrap_reference< System >::type system_type;
        do_step_impl( system , in , t , out , dt , typename is_pair< system_type >::type() );
    }

    template< class StateType >
    void adjust_size( const StateType &x )
    {
        resize_dqdt( x );
        resize_dpdt( x );
    }

    /** \brief Returns the coefficients a. */
    const coef_type& coef_a( void ) const { return m_coef_a; }

    /** \brief Returns the coefficients b. */
    const coef_type& coef_b( void ) const { return m_coef_b; }

private:

    // stepper for systems with function for dq/dt = f(p) and dp/dt = -f(q)
    template< class System , class StateIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , time_type /* t */ , StateOut &out , time_type dt , boost::mpl::true_ )
    {
        typedef typename odeint::unwrap_reference< System >::type system_type;
        typedef typename odeint::unwrap_reference< typename system_type::first_type >::type coor_deriv_func_type;
        typedef typename odeint::unwrap_reference< typename system_type::second_type >::type momentum_deriv_func_type;
        system_type &sys = system;
        coor_deriv_func_type &coor_func = sys.first;
        momentum_deriv_func_type &momentum_func = sys.second;

        typedef typename odeint::unwrap_reference< StateIn >::type state_in_type;
        typedef typename odeint::unwrap_reference< typename state_in_type::first_type >::type coor_in_type;
        typedef typename odeint::unwrap_reference< typename state_in_type::second_type >::type momentum_in_type;
        const state_in_type &state_in = in;
        const coor_in_type &coor_in = state_in.first;
        const momentum_in_type &momentum_in = state_in.second;

        typedef typename odeint::unwrap_reference< StateOut >::type state_out_type;
        typedef typename odeint::unwrap_reference< typename state_out_type::first_type >::type coor_out_type;
        typedef typename odeint::unwrap_reference< typename state_out_type::second_type >::type momentum_out_type;
        state_out_type &state_out = out;
        coor_out_type &coor_out = state_out.first;
        momentum_out_type &momentum_out = state_out.second;

        m_dqdt_resizer.adjust_size( coor_in , detail::bind( &internal_stepper_base_type::template resize_dqdt< coor_in_type > , detail::ref( *this ) , detail::_1 ) );
        m_dpdt_resizer.adjust_size( momentum_in , detail::bind( &internal_stepper_base_type::template resize_dpdt< momentum_in_type > , detail::ref( *this ) , detail::_1 ) );

        // ToDo: check sizes?

        for( size_t l=0 ; l<num_of_stages ; ++l )
        {
            if( l == 0 )
            {
                coor_func( momentum_in , m_dqdt.m_v );
                this->m_algebra.for_each3( coor_out , coor_in , m_dqdt.m_v ,
                        typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
                momentum_func( coor_out , m_dpdt.m_v );
                this->m_algebra.for_each3( momentum_out , momentum_in , m_dpdt.m_v ,
                        typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
            }
            else
            {
                coor_func( momentum_out , m_dqdt.m_v );
                this->m_algebra.for_each3( coor_out , coor_out , m_dqdt.m_v ,
                        typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
                momentum_func( coor_out , m_dpdt.m_v );
                this->m_algebra.for_each3( momentum_out , momentum_out , m_dpdt.m_v ,
                        typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
            }
        }
    }

    // stepper for systems with only function dp /dt = -f(q), dq/dt = p, time not required but still expected for compatibility reasons
    template< class System , class StateIn , class StateOut >
    void do_step_impl( System system , const StateIn &in , time_type  /* t */ , StateOut &out , time_type dt , boost::mpl::false_ )
    {
        typedef typename odeint::unwrap_reference< System >::type momentum_deriv_func_type;
        momentum_deriv_func_type &momentum_func = system;

        typedef typename odeint::unwrap_reference< StateIn >::type state_in_type;
        typedef typename odeint::unwrap_reference< typename state_in_type::first_type >::type coor_in_type;
        typedef typename odeint::unwrap_reference< typename state_in_type::second_type >::type momentum_in_type;
        const state_in_type &state_in = in;
        const coor_in_type &coor_in = state_in.first;
        const momentum_in_type &momentum_in = state_in.second;

        typedef typename odeint::unwrap_reference< StateOut >::type state_out_type;
        typedef typename odeint::unwrap_reference< typename state_out_type::first_type >::type coor_out_type;
        typedef typename odeint::unwrap_reference< typename state_out_type::second_type >::type momentum_out_type;
        state_out_type &state_out = out;
        coor_out_type &coor_out = state_out.first;
        momentum_out_type &momentum_out = state_out.second;

        // m_dqdt not required when called with momentum_func only - don't resize
        // m_dqdt_resizer.adjust_size( coor_in , detail::bind( &internal_stepper_base_type::template resize_dqdt< coor_in_type > , detail::ref( *this ) , detail::_1 ) );
        m_dpdt_resizer.adjust_size( momentum_in , detail::bind( &internal_stepper_base_type::template resize_dpdt< momentum_in_type > , detail::ref( *this ) , detail::_1 ) );

        // ToDo: check sizes?

        // step 0
        this->m_algebra.for_each3( coor_out  , coor_in , momentum_in ,
                        typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[0] * dt ) );
        momentum_func( coor_out , m_dpdt.m_v );
        this->m_algebra.for_each3( momentum_out , momentum_in , m_dpdt.m_v ,
                                           typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[0] * dt ) );

        for( size_t l=1 ; l<num_of_stages ; ++l )
        {
            this->m_algebra.for_each3( coor_out , coor_out , momentum_out ,
                        typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_a[l] * dt ) );
            momentum_func( coor_out , m_dpdt.m_v );
            this->m_algebra.for_each3( momentum_out , momentum_out , m_dpdt.m_v ,
                                       typename operations_type::template scale_sum2< value_type , time_type >( 1.0 , m_coef_b[l] * dt ) );
        }
    }

    template< class StateIn >
    bool resize_dqdt( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dqdt , x , typename is_resizeable<coor_deriv_type>::type() );
    }

    template< class StateIn >
    bool resize_dpdt( const StateIn &x )
    {
        return adjust_size_by_resizeability( m_dpdt , x , typename is_resizeable<momentum_deriv_type>::type() );
    }

    const coef_type m_coef_a;
    const coef_type m_coef_b;

    resizer_type m_dqdt_resizer;
    resizer_type m_dpdt_resizer;
    wrapped_coor_deriv_type m_dqdt;
    wrapped_momentum_deriv_type m_dpdt;

};

/********* DOXYGEN *********/

/**
 * \class symplectic_nystroem_stepper_base
 * \brief Base class for all symplectic steppers of Nystroem type.
 *
 * This class is the base class for the symplectic Runge-Kutta-Nystroem steppers. Symplectic steppers are usually
 * used to solve Hamiltonian systems and they conserve the phase space volume, see
 * en.wikipedia.org/wiki/Symplectic_integrator.
 * Furthermore, the energy is conserved
 * in average. In detail this class of steppers can be used to solve separable Hamiltonian systems which can be written
 * in the form H(q,p) = H1(p) + H2(q). q is usually called the coordinate, while p is the momentum. The equations of motion
 * are dq/dt = dH1/dp, dp/dt = -dH2/dq.
 *
 * ToDo : add formula for solver and explanation of the coefficients
 *
 * symplectic_nystroem_stepper_base uses odeints algebra and operation system. Step size and error estimation are not
 * provided for this class of solvers. It derives from algebra_stepper_base. Several `do_step` variants are provided:
 *
 * - `do_step( sys , x , t , dt )` - The classical `do_step` method. The sys can be either a pair of function objects
 *    for the coordinate or the momentum part or one function object for the momentum part. `x` is a pair of coordinate
 *    and momentum. The state is updated in-place.
 * - `do_step( sys , q , p , t , dt )` - This method is similar to the method above with the difference that the coordinate
 *    and the momentum are passed explicitly and not packed into a pair.
 * - `do_step( sys , x_in , t , x_out , dt )` - This method transforms the state out-of-place. `x_in` and `x_out` are here pairs
 *    of coordinate and momentum.
 *
 * \tparam NumOfStages Number of stages.
 * \tparam Order The order of the stepper.
 * \tparam Coor The type representing the coordinates q.
 * \tparam Momentum The type representing the coordinates p.
 * \tparam Value The basic value type. Should be something like float, double or a high-precision type.
 * \tparam CoorDeriv The type representing the time derivative of the coordinate dq/dt.
 * \tparam MomemtnumDeriv The type representing the time derivative of the momentum dp/dt.
 * \tparam Time The type representing the time t.
 * \tparam Algebra The algebra.
 * \tparam Operations The operations.
 * \tparam Resizer The resizer policy.
 */

    /**
     * \fn symplectic_nystroem_stepper_base::symplectic_nystroem_stepper_base( const coef_type &coef_a , const coef_type &coef_b , const algebra_type &algebra )
     * \brief Constructs a symplectic_nystroem_stepper_base class. The parameters of the specific Nystroem method and the
     * algebra have to be passed.
     * \param coef_a The coefficients a.
     * \param coef_b The coefficients b.
     * \param algebra A copy of algebra is made and stored inside explicit_stepper_base.
     */

    /**
     * \fn symplectic_nystroem_stepper_base::order( void ) const
     * \return Returns the order of the stepper.
     */

    /**
     * \fn symplectic_nystroem_stepper_base::do_step( System system , const StateInOut &state , time_type t , time_type dt )
     * \brief This method performs one step. The system can be either a pair of two function object
     * describing the momentum part and the coordinate part or one function object describing only
     * the momentum part. In this case the coordinate is assumed to be trivial dq/dt = p. The state
     * is updated in-place.
     *
     * \note boost::ref or std::ref can be used for the system as well as for the state. So, it is correct
     * to write `stepper.do_step( make_pair( std::ref( fq ) , std::ref( fp ) ) , make_pair( std::ref( q ) , std::ref( p ) ) , t , dt )`.
     *
     * \note This method solves the forwarding problem.
     *
     * \param system The system, can be represented as a pair of two function object or one function object. See above.
     * \param state The state of the ODE. It is a pair of Coor and Momentum. The state is updated in-place, therefore, the
     * new value of the state will be written into this variable.
     * \param t The time of the ODE. It is not advanced by this method.
     * \param dt The time step.
     */

    /**
     * \fn symplectic_nystroem_stepper_base::do_step( System system , CoorInOut &q , MomentumInOut &p , time_type t , time_type dt )
     * \brief This method performs one step. The system can be either a pair of two function object
     * describing the momentum part and the coordinate part or one function object describing only
     * the momentum part. In this case the coordinate is assumed to be trivial dq/dt = p. The state
     * is updated in-place.
     *
     * \note boost::ref or std::ref can be used for the system. So, it is correct
     * to write `stepper.do_step( make_pair( std::ref( fq ) , std::ref( fp ) ) , q , p , t , dt )`.
     *
     * \note This method solves the forwarding problem.
     *
     * \param system The system, can be represented as a pair of two function object or one function object. See above.
     * \param q The coordinate of the ODE. It is updated in-place. Therefore, the new value of the coordinate will be written
     * into this variable.
     * \param p The momentum of the ODE. It is updated in-place. Therefore, the new value of the momentum will be written info
     * this variable.
     * \param t The time of the ODE. It is not advanced by this method.
     * \param dt The time step.
     */

    /**
     * \fn symplectic_nystroem_stepper_base::do_step( System system , const StateIn &in , time_type t , StateOut &out , time_type dt )
     * \brief This method performs one step. The system can be either a pair of two function object
     * describing the momentum part and the coordinate part or one function object describing only
     * the momentum part. In this case the coordinate is assumed to be trivial dq/dt = p. The state
     * is updated out-of-place.
     *
     * \note boost::ref or std::ref can be used for the system. So, it is correct
     * to write `stepper.do_step( make_pair( std::ref( fq ) , std::ref( fp ) ) , x_in , t , x_out , dt )`.
     *
     * \note This method NOT solve the forwarding problem.
     *
     * \param system The system, can be represented as a pair of two function object or one function object. See above.
     * \param in The state of the ODE, which is a pair of coordinate and momentum. The state is updated out-of-place, therefore the
     * new value is written into out
     * \param t The time of the ODE. It is not advanced by this method.
     * \param out The new state of the ODE.
     * \param dt The time step.
     */

    /**
     * \fn symplectic_nystroem_stepper_base::adjust_size( const StateType &x )
     * \brief Adjust the size of all temporaries in the stepper manually.
     * \param x A state from which the size of the temporaries to be resized is deduced.
     */

} // namespace odeint
} // namespace numeric
} // namespace boost

#endif // BOOST_NUMERIC_ODEINT_STEPPER_BASE_SYMPLECTIC_RKN_STEPPER_BASE_HPP_INCLUDED


//==================================================================================================
/*!
  @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_ARCH_COMMON_SIMD_FUNCTION_BITOFSIGN_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SIMD_FUNCTION_BITOFSIGN_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/meta/hierarchy/simd.hpp>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/constant/mzero.hpp>
#include <boost/simd/constant/signmask.hpp>
#include <boost/simd/constant/zero.hpp>

namespace boost { namespace simd { namespace ext
{
   namespace bd = boost::dispatch;
   namespace bs = boost::simd;
   BOOST_DISPATCH_OVERLOAD(bitofsign_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::signed_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return bitwise_and(a0, bs::Signmask<A0>());
      }
   };

   BOOST_DISPATCH_OVERLOAD(bitofsign_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::arithmetic_<A0>, X>
                          )
   {
     BOOST_FORCEINLINE A0 operator()(const A0&)const
      {
        return Zero<A0>();
      }
   };

   BOOST_DISPATCH_OVERLOAD(bitofsign_
                          , (typename A0, typename X)
                          , bd::cpu_
                          , bs::pack_<bd::floating_<A0>, X>
                          )
   {
      BOOST_FORCEINLINE A0 operator()( const A0& a0) const BOOST_NOEXCEPT
      {
        return bitwise_and(a0, bs::Mzero<A0>());
      }
   };

} } }

#endif


// Copyright Louis Dionne 2013-2016
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)

#include <boost/hana/ext/boost/mpl/integral_c.hpp>

#include <boost/hana/tuple.hpp>
#include <boost/hana/value.hpp>

#include <laws/constant.hpp>

#include <boost/mpl/int.hpp>
#include <boost/mpl/integral_c.hpp>
namespace hana = boost::hana;
namespace mpl = boost::mpl;

int main() {
    // value
    static_assert(hana::value(mpl::integral_c<int, 0>{}) == 0, "");
    static_assert(hana::value(mpl::integral_c<int, 1>{}) == 1, "");
    static_assert(hana::value(mpl::integral_c<int, 3>{}) == 3, "");

    // laws
    hana::test::TestConstant<hana::ext::boost::mpl::integral_c_tag<int>>{
        hana::make_tuple(
            mpl::int_<-10>{}, mpl::int_<-2>{}, mpl::integral_c<int, 0>{},
            mpl::integral_c<int, 1>{}, mpl::integral_c<int, 3>{}
        ),
        hana::tuple_t<int, long, long long>
    };
}


#pragma once

#include <Eigen/Core>
#include <Eigen/SparseCore>
#include <vector>

#include <opt/optimization_problem.hpp>

namespace ipc::rigid {

class ConstrainedProblem : public virtual OptimizationProblem {
public:
    virtual ~ConstrainedProblem() = default;

    /// Compute the constraint function g(x) ≥ 0
    virtual void compute_constraints(
        const Eigen::VectorXd& x,
        Eigen::VectorXd& gx,
        Eigen::MatrixXd& jac_gx,
        std::vector<Eigen::SparseMatrix<double>>& hess_gx,
        bool compute_grad = true,
        bool compute_hess = true) = 0;

    /// Compute the constraint function g(x) ≥ 0
    virtual void compute_constraints_using_normals(
        const Eigen::VectorXd& x,
        Eigen::VectorXd& gx,
        Eigen::MatrixXd& jac_gx) = 0;

    // --------------------------------------------------------------------
    // Convience functions
    // --------------------------------------------------------------------

    virtual void
    compute_constraints(const Eigen::VectorXd& x, Eigen::VectorXd& gx) final
    {
        Eigen::MatrixXd jac_gx;
        std::vector<Eigen::SparseMatrix<double>> hess_gx;
        return compute_constraints(
            x, gx, jac_gx, hess_gx,
            /*compute_grad=*/false, /*compute_hess=*/false);
    }

    virtual void compute_constraints(
        const Eigen::VectorXd& x,
        Eigen::VectorXd& gx,
        Eigen::MatrixXd& jac_gx) final
    {
        std::vector<Eigen::SparseMatrix<double>> hess_gx;
        return compute_constraints(
            x, gx, jac_gx, hess_gx,
            /*compute_grad=*/true, /*compute_hess=*/false);
    }

    virtual bool is_constrained_problem() const { return true; }
};

} // namespace ipc::rigid


//---------------------------------------------------------------------------//
//!
//! \file   Utility_BasicBivariateDistribution.hpp
//! \author Alex Robinson
//! \brief  The basic bivariate distribution class declaration
//!
//---------------------------------------------------------------------------//

#ifndef UTILITY_BASIC_BIVARIATE_DISTRIBUTION_HPP
#define UTILITY_BASIC_BIVARIATE_DISTRIBUTION_HPP

// Std Lib Includes
#include <stdexcept>

// Boost Includes
#include <boost/serialization/split_member.hpp>

// FRENSIE Includes
#include "Utility_OStreamableObject.hpp"
#include "Utility_UnitTraits.hpp"
#include "Utility_QuantityTraits.hpp"
#include "Utility_DistributionTraits.hpp"
#include "Utility_DistributionSerializationHelpers.hpp"

/*! \defgroup bivariate_distributions Bivariate Distributions
 */

namespace Utility{

/*! The unit-aware basic bivariate distribution
 * \ingroup bivariate_distributions
 */
template<typename PrimaryIndependentUnit,
         typename SecondaryIndependentUnit,
         typename DependentUnit>
class UnitAwareBasicBivariateDistribution : public OStreamableObject
{

  // Typedef for this type
  typedef UnitAwareBasicBivariateDistribution<PrimaryIndependentUnit,SecondaryIndependentUnit,DependentUnit> ThisType;

protected:

  //! The primary independent unit traits typedef
  typedef UnitTraits<PrimaryIndependentUnit> PrimaryIndepUnitTraits;

  //! The secondary independent unit traits typedef
  typedef UnitTraits<SecondaryIndependentUnit> SecondaryIndepUnitTraits;

  //! The inverse primary independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<PrimaryIndependentUnit>::InverseUnit> InversePrimaryIndepUnitTraits;

  //! The inverse secondary independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<SecondaryIndependentUnit>::InverseUnit> InverseSecondaryIndepUnitTraits;

  //! The inverse independent unit traits typedef
  typedef UnitTraits<typename UnitTraits<typename UnitTraits<PrimaryIndependentUnit>::InverseUnit>::template GetMultipliedUnitType<typename UnitTraits<SecondaryIndependentUnit>::InverseUnit>::type> InverseIndepUnitTraits;

  //! The dependent unit traits typedef
  typedef UnitTraits<DependentUnit> DepUnitTraits;

public:

  //! The primary independent unit type
  typedef PrimaryIndependentUnit PrimaryIndepUnit;

  //! The secondary independent unit type
  typedef SecondaryIndependentUnit SecondaryIndepUnit;

  //! The dependent unit type
  typedef DependentUnit DepUnit;

  //! The primary independent quantity type
  typedef typename PrimaryIndepUnitTraits::template GetQuantityType<double>::type PrimaryIndepQuantity;

  //! The secondary independent quantity type
  typedef typename SecondaryIndepUnitTraits::template GetQuantityType<double>::type SecondaryIndepQuantity;

  //! The inverse secondary independent quantity type
  typedef typename InverseSecondaryIndepUnitTraits::template GetQuantityType<double>::type InverseSecondaryIndepQuantity;

  //! The dependent quantity type
  typedef typename DepUnitTraits::template GetQuantityType<double>::type DepQuantity;

  //! Constructor
  UnitAwareBasicBivariateDistribution()
  { /* ... */ }

  //! Destructor
  virtual ~UnitAwareBasicBivariateDistribution()
  { /* ... */ }

  //! Evaluate the distribution
  virtual DepQuantity evaluate(
            const PrimaryIndepQuantity primary_indep_var_value,
            const SecondaryIndepQuantity secondary_indep_var_value ) const = 0;

  //! Evaluate the secondary conditional PDF
  virtual InverseSecondaryIndepQuantity evaluateSecondaryConditionalPDF(
            const PrimaryIndepQuantity primary_indep_var_value,
            const SecondaryIndepQuantity secondary_indep_var_value ) const = 0;

  //! Return a random sample from the secondary conditional PDF
  virtual SecondaryIndepQuantity sampleSecondaryConditional(
                const PrimaryIndepQuantity primary_indep_var_value ) const = 0;

  //! Return a random sample and record the number of trials
  virtual SecondaryIndepQuantity sampleSecondaryConditionalAndRecordTrials(
                            const PrimaryIndepQuantity primary_indep_var_value,
                            DistributionTraits::Counter& trials ) const = 0;

  //! Return the upper bound of the distribution primary independent variable
  virtual PrimaryIndepQuantity getUpperBoundOfPrimaryIndepVar() const = 0;

  //! Return the lower bound of the distribution primary independent variable
  virtual PrimaryIndepQuantity getLowerBoundOfPrimaryIndepVar() const = 0;

  //! Return the upper bound of the secondary conditional distribution
  virtual SecondaryIndepQuantity getUpperBoundOfSecondaryConditionalIndepVar(
                const PrimaryIndepQuantity primary_indep_var_value ) const = 0;

  //! Return the lower bound of the secondary conditional distribution
  virtual SecondaryIndepQuantity getLowerBoundOfSecondaryConditionalIndepVar(
                const PrimaryIndepQuantity primary_indep_var_value ) const = 0;

  //! Test if the distribution is tabular in the primary dimension
  virtual bool isPrimaryDimensionTabular() const = 0;

  //! Test if the distribution is continuous in the primary dimension
  virtual bool isPrimaryDimensionContinuous() const = 0;

  //! Test if the distribution has the same primary bounds
  bool hasSamePrimaryBounds( const UnitAwareBasicBivariateDistribution& distribution ) const;

protected:

  //! Add distribution data to the stream
  template<typename... Types>
  void toStreamDistImpl( std::ostream& os,
                         const std::string& name,
                         const Types&... data ) const;

private:

  // Save the distribution to an archive
  template<typename Archive>
  void save( Archive& ar, const unsigned version ) const
  { /* ... */ }

  // Load the distribution from an archive
  template<typename Archive>
  void load( Archive& ar, const unsigned version )
  { /* ... */ }

  BOOST_SERIALIZATION_SPLIT_MEMBER();

  // Declare the boost serialization access object as a friend
  friend class boost::serialization::access;
};

/*! The basic bivariate distribution (unit-agnostic)
 * \ingroup bivariate_distributions
 */
typedef UnitAwareBasicBivariateDistribution<void,void,void> BasicBivariateDistribution;

/*! \brief Exception thrown by BivariateDistribution objects when an invalid
 * parameter is encountered.
 * \ingroup bivariate_distributions
 */
class BadBivariateDistributionParameter : public std::logic_error
{
public:
  BadBivariateDistributionParameter( const std::string& msg )
    : std::logic_error( msg )
  { /* ... */ }

  ~BadBivariateDistributionParameter() throw()
  { /* ... */ }
};

} // end Utility namespace

BOOST_SERIALIZATION_ASSUME_ABSTRACT_DISTRIBUTION3( UnitAwareBasicBivariateDistribution );
BOOST_SERIALIZATION_DISTRIBUTION3_VERSION( UnitAwareBasicBivariateDistribution, 0 );

//---------------------------------------------------------------------------//
// Template Includes
//---------------------------------------------------------------------------//

#include "Utility_BasicBivariateDistribution_def.hpp"

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

#endif // end UTILITY_BASIC_BIVARIATE_DISTRIBUTION_HPP

//---------------------------------------------------------------------------//
// end Utility_BivariateDistribution.hpp
//---------------------------------------------------------------------------//


//==============================================================================
//         Copyright 2003 - 2012   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   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
//==============================================================================

#include <nt2/include/functions/plinsolve.hpp>
#include <nt2/include/functions/gesv.hpp>
#include <nt2/include/functions/eye.hpp>
#include <nt2/include/functions/zeros.hpp>
#include <nt2/include/functions/ones.hpp>
#include <nt2/include/functions/width.hpp>
#include <nt2/include/functions/height.hpp>
#include <nt2/include/functions/mtimes.hpp>
#include <nt2/include/functions/cons.hpp>
#include <boost/core/ignore_unused.hpp>

#include <nt2/table.hpp>

#include <nt2/sdk/unit/module.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>

NT2_TEST_CASE_TPL(plinsolve, NT2_REAL_TYPES )
{
using nt2::_;

typedef nt2::table<T>           t_t;
typedef nt2::table<nt2_la_int>  t_i;

t_t lu;

t_t a = nt2::cons<T>(nt2::of_size(3,3),2,1,1,1,2,2,2,5,7);
t_t b = nt2::cons<T>(nt2::of_size(3,1),1,2,5);
t_t x = nt2::ones(nt2::of_size(10,1),nt2::meta::as_<T>());
t_t x1(b+b);
t_i piv;

nt2::plinsolve(a ,b+b, nt2::tie(x) );
nt2_la_int iter = nt2::gesv( boost::proto::value(a), boost::proto::value(piv)
                         , boost::proto::value(x1) );

NT2_TEST_ULP_EQUAL(x(_(1,3)), x1 , T(10));
NT2_TEST_EQUAL(T(iter)>=T(0)?true:false, true);

}


#ifndef MP_FOLLOWING_GRAPH_HPP
#define MP_FOLLOWING_GRAPH_HPP

#include <unordered_map>

#include "defs.hpp"
#include "serialization.hpp"

#include <boost/graph/adjacency_list.hpp>

namespace mp {

struct following_data;

struct vertex_data
{
    string name; // unique device ID
};

struct edge_data
{
    double weight = 0.0;
};

/**
 * A graph of devices and following relations.
 */
using following_graph = boost::adjacency_list<
    boost::vecS,
    boost::vecS,
    boost::bidirectionalS,  // edges are directed and can be visited from both directions
    vertex_data,            // data stored with each vertex
    edge_data               // data stored with each edge
>;

/**
 * Returns the following graph for the given following_data at `timestamp`.
 *
 * The function adds a vertex for every device that is mentioned by the data
 * at that timestamp.
 * An edge is added between two vertices a, b if a is following b.
 * If the relation between a and b is "co-leading", both edges (a, b) and (b, a)
 * will be created.
 *
 * \param f the following data to use.
 * \param timestamp must be in range.
 */
following_graph following_graph_at(const following_data &f, i64 timestamp);

/**
 * Stores the list of leaders for every timestamp.
 */
struct leader_data
{
    struct timestamp_data
    {
        i64 timestamp = 0;
        vector<string> leaders;
    };

    /**
     * Returns the data for the given timestamp.
     */
    const timestamp_data& data_at(i64 timestamp) const
    {
        assert(timestamp >= begin_timestamp
               && timestamp <= end_timestamp
               && "Timestamp in range");
        size_t index = static_cast<size_t>(timestamp - begin_timestamp);
        return timestamps[index];
    }

    /**
     * Returns the data for the given timestamp.
     */
    timestamp_data& data_at(i64 timestamp)
    {
        assert(timestamp >= begin_timestamp
               && timestamp <= end_timestamp
               && "Timestamp in range");
        size_t index = static_cast<size_t>(timestamp - begin_timestamp);
        return timestamps[index];
    }

    i64 begin_timestamp = 0;
    i64 end_timestamp = 0;
    i64 duration = 0;
    vector<string> devices; // All devices
    vector<timestamp_data> timestamps;
};

/**
 * Detect leaders in the given graph using the PageRank algorithm.
 *
 * The computation works as follows:
 * - Run the PageRank algorithm over the entire graph
 * - Find the set of connected components in the given graph.
 *   Every such component is considered a group.
 * - For every connected component, find the node with the hightest PageRank value.
 *   This node is classified as a leader of its group.
 *
 * This function will return the list of these leaders.
 */
vector<string> detect_leaders(const following_graph &g, bool use_weights);

/**
 * Detect groups in the given graph using the connected-components algorithm.
 *
 * This function will return a vector of vectors.
 * Each inner vector stores a single group and contains the names of all
 * members.
 */
vector<vector<string>> detect_groups(const following_graph &g);

/**
 * Same as `detect_leaders(following_graph)`, but for all timestamps in `fd`.
 */
leader_data detect_leaders(const following_data &fd, bool use_weights);

/**
 *  Writes the graph to the given output stream, using the GraphML format.
 */
void to_graphml(const following_graph &g, std::ostream &o);

struct serialized_vertex
{
    string id;

    template<typename Archive>
    void serialize(Archive &ar)
    {
        ar(cereal::make_nvp("id", id));
    }
};

struct serialized_edge
{
    string source;
    string target;
    double weight;

    template<typename Archive>
    void serialize(Archive &ar)
    {
        ar(cereal::make_nvp("source", source),
           cereal::make_nvp("target", target),
           cereal::make_nvp("weight", weight));
    }
};

/**
 * Save the graph using the given archive.
 */
template<typename Archive>
void save(Archive &ar, const following_graph &g)
{
    vector<serialized_vertex> vertex_list;
    vector<serialized_edge> edge_list;

    for (auto v : make_iter_range(vertices(g))) {
        vertex_list.push_back({g[v].name});
    }
    for (auto e : make_iter_range(edges(g))) {
        auto s = source(e, g);
        auto t = target(e, g);
        edge_list.push_back({g[s].name, g[t].name, g[e].weight});
    }

    ar(cereal::make_nvp("vertices", vertex_list),
       cereal::make_nvp("edges", edge_list));
}

/**
 * Load the graph using the given archive.
 */
template<typename Archive>
void load(Archive &ar, following_graph &g)
{
    vector<serialized_vertex> vertex_list;
    vector<serialized_edge> edge_list;
    ar(cereal::make_nvp("vertices", vertex_list),
       cereal::make_nvp("edges", edge_list));

    using vertex_map = std::unordered_map<string, following_graph::vertex_descriptor>;

    vertex_map v; // vertex name -> vertex descriptor
    auto get_vertex = [&](const string &id) {
        auto iter = v.find(id);
        if (iter == v.end()) {
            throw std::logic_error("serialized graph misses vertex: " + id);
        }
        return iter->second;
    };

    g.clear();
    for (auto &vertex : vertex_list) {
        vertex_data d{vertex.id};
        v[vertex.id] = add_vertex(d, g);
    }
    for (auto &edge : edge_list) {
        auto s = get_vertex(edge.source);
        auto t = get_vertex(edge.target);

        edge_data d;
        d.weight = edge.weight;
        add_edge(s, t, d, g);
    }
}

/**
 * Serialize the timestamp data using the given archive.
 *
 * \relates leader_data::timestamp_data
 */
template<typename Archive>
void serialize(Archive &ar, leader_data::timestamp_data &td)
{
    ar(cereal::make_nvp("timestamp", td.timestamp),
       cereal::make_nvp("leaders", td.leaders));
}

/**
 * Serialize the leader data using the given archive.
 *
 * \relates leader_data
 */
template<typename Archive>
void serialize(Archive &ar, leader_data &ld)
{
    ar(cereal::make_nvp("begin_timestamp", ld.begin_timestamp),
       cereal::make_nvp("end_timestamp", ld.end_timestamp),
       cereal::make_nvp("duration", ld.duration),
       cereal::make_nvp("devices", ld.devices),
       cereal::make_nvp("timestamps", ld.timestamps));
}

} // namespace mp

#endif // MP_FOLLOWING_GRAPH_HPP


/*
@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_NUMERIC_HPP
#define BOOST_HANA_TEST_TEST_NUMERIC_HPP

#include <boost/hana/core/tag_of.hpp>
#include <boost/hana/eval.hpp>

// instances
#include <boost/hana/concept/comparable.hpp>
#include <boost/hana/concept/euclidean_ring.hpp>
#include <boost/hana/concept/group.hpp>
#include <boost/hana/concept/logical.hpp>
#include <boost/hana/concept/monoid.hpp>
#include <boost/hana/concept/orderable.hpp>
#include <boost/hana/concept/ring.hpp>

namespace boost { namespace hana {
    namespace test {
        struct numeric_type {
            constexpr explicit numeric_type(int v) : value(v) { }
            int value;
            constexpr operator int() const { return value; }
        };

        using Numeric = tag_of_t<numeric_type>;

        struct numeric_t {
            constexpr numeric_type operator()(int x) const {
                return numeric_type{x};
            }
        };
        constexpr numeric_t numeric{};
    }

    //////////////////////////////////////////////////////////////////////////
    // Comparable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct equal_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value == y.value); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Orderable
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct less_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y) {
            // Workaround a _weird_ GCC bug:
            // error: parse error in template argument list
            //      bool cmp = (x.value < y.value);
            //                    ^
            int xv = x.value, yv = y.value;
            return test::numeric(xv < yv);
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Logical
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct eval_if_impl<test::Numeric> {
        template <typename C, typename T, typename E>
        static constexpr auto apply(C c, T t, E e) {
            return c.value ? hana::eval(t) : hana::eval(e);
        }
    };

    template <>
    struct not_impl<test::Numeric> {
        template <typename X>
        static constexpr auto apply(X x)
        { return test::numeric(!x.value); }
    };

    template <>
    struct while_impl<test::Numeric> {
        template <typename Pred, typename State, typename F>
        static constexpr auto apply(Pred pred, State state, F f)
            -> decltype(true ? f(state) : state)
        {
            if (pred(state))
                return while_(pred, f(state), f);
            else
                return state;
        }
    };

    //////////////////////////////////////////////////////////////////////////
    // Monoid
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct plus_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value + y.value); }
    };

    template <>
    struct zero_impl<test::Numeric> {
        static constexpr auto apply()
        { return test::numeric(0); }
    };

    //////////////////////////////////////////////////////////////////////////
    // Group
    //
    // Define either one to select which MCD is used:
    //  BOOST_HANA_TEST_GROUP_NEGATE_MCD
    //  BOOST_HANA_TEST_GROUP_MINUS_MCD
    //
    // If neither is defined, the MCD used is unspecified.
    //////////////////////////////////////////////////////////////////////////
#if defined(BOOST_HANA_TEST_GROUP_NEGATE_MCD)
    template <>
    struct negate_impl<test::Numeric> {
        template <typename X>
        static constexpr auto apply(X x)
        { return test::numeric(-x.value); }
    };
#else
    template <>
    struct minus_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value - y.value); }
    };
#endif

    //////////////////////////////////////////////////////////////////////////
    // Ring
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct mult_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value * y.value); }
    };

    template <>
    struct one_impl<test::Numeric> {
        static constexpr auto apply()
        { return test::numeric(1); }
    };

    //////////////////////////////////////////////////////////////////////////
    // EuclideanRing
    //////////////////////////////////////////////////////////////////////////
    template <>
    struct div_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value / y.value); }
    };

    template <>
    struct mod_impl<test::Numeric, test::Numeric> {
        template <typename X, typename Y>
        static constexpr auto apply(X x, Y y)
        { return test::numeric(x.value % y.value); }
    };
}} // end namespace boost::hana

#endif //! BOOST_HANA_TEST_TEST_NUMERIC_HPP


#include "../schnorr_proof.h"
#include "../schnorr_prover.h"
#include "../schnorr_verifier.h"
#include "../challenge_generator_impl.h"
#include "../../streams.h"
#include "../../version.h"

#include <boost/test/unit_test.hpp>

namespace lelantus {

class SchnorrProofTests {
public:
    SchnorrProofTests()
    {
        g.randomize();
        h.randomize();
        P.randomize();
        T.randomize();
        a.randomize();
        b.randomize();
    }

public:
    GroupElement g, h, a, b;
    Scalar P, T;
};

BOOST_FIXTURE_TEST_SUITE(lelantus_schnorr_proof_tests, SchnorrProofTests)

BOOST_AUTO_TEST_CASE(serialization)
{
    std::unique_ptr<ChallengeGenerator> challengeGenerator = std::make_unique<ChallengeGeneratorImpl<CHash256>>(1);
    SchnorrProver prover(g, h, true);
    GroupElement y;
    y.randomize();
    SchnorrProof proof;
    prover.proof(P, T, y, a, b, challengeGenerator, proof);

    CDataStream serialized(SER_NETWORK, PROTOCOL_VERSION);
    serialized << proof;

    SchnorrProof deserialized;
    serialized >> deserialized;

    BOOST_CHECK(proof.u == deserialized.u);
    BOOST_CHECK(proof.P1 == deserialized.P1);
    BOOST_CHECK(proof.T1 == deserialized.T1);
}

BOOST_AUTO_TEST_CASE(prove_verify)
{
    auto y = LelantusPrimitives::commit(g, P, h, T);
    std::unique_ptr<ChallengeGenerator> challengeGenerator = std::make_unique<ChallengeGeneratorImpl<CHash256>>(1);

    SchnorrProver prover(g, h, true);
    SchnorrProof proof;
    prover.proof(P, T, y, a, b, challengeGenerator, proof);

    SchnorrVerifier verifier(g, h, true);
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(verifier.verify(y, a, b, proof, challengeGenerator));
}

BOOST_AUTO_TEST_CASE(fake_prove_not_verify)
{
    auto y = LelantusPrimitives::commit(g, P, h, T);
    std::unique_ptr<ChallengeGenerator> challengeGenerator = std::make_unique<ChallengeGeneratorImpl<CHash256>>(1);

    SchnorrProver prover(g, h, true);
    SchnorrProof proof;
    prover.proof(P, T, y, a, b, challengeGenerator, proof);

    GroupElement fakeY;
    fakeY.randomize();

    SchnorrVerifier verifier(g, h, true);
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(!verifier.verify(fakeY, a, b, proof, challengeGenerator));

    auto fakeProof = proof;
    fakeProof.P1.randomize();
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(!verifier.verify(y, a, b, fakeProof, challengeGenerator));

    fakeProof = proof;
    fakeProof.T1.randomize();
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(!verifier.verify(y, a, b, fakeProof, challengeGenerator));

    fakeProof = proof;
    fakeProof.u.randomize();
    challengeGenerator.reset(new ChallengeGeneratorImpl<CHash256>(1));
    BOOST_CHECK(!verifier.verify(y, a, b, fakeProof, challengeGenerator));
}

BOOST_AUTO_TEST_SUITE_END()

} // namespace lelantus


#define OFDIS_INTERNAL
#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Dense>
#include "litiv/3rdparty/ofdis/fdf/image.h"
#include "litiv/3rdparty/ofdis/patch.hpp"
#include "litiv/3rdparty/ofdis/patchgrid.hpp"
#include "litiv/utils/defines.hpp" // only used here for compiler flags

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
ofdis::PatGridClass<eInput,eOutput>::PatGridClass(
        const camparam* cpt_in,
        const camparam* cpo_in,
        const optparam* op_in) :
        cpt(cpt_in),
        cpo(cpo_in),
        op(op_in) {
    // Generate grid on current scale
    steps = op->steps;
    nopw = ceil((float)cpt->width/(float)steps);
    noph = ceil((float)cpt->height/(float)steps);
    const int offsetw = floor((cpt->width-(nopw-1)*steps)/2);
    const int offseth = floor((cpt->height-(noph-1)*steps)/2);
    nopatches = nopw*noph;
    pt_ref.resize(nopatches);
    p_init.resize(nopatches);
    pat.reserve(nopatches);
    im_ao_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_ao_dx_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_ao_dy_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_bo_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_bo_dx_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    im_bo_dy_eg = new Eigen::Map<const Eigen::MatrixXf>(nullptr,cpt->height,cpt->width);
    int patchid = 0;
    for(int x=0; x<nopw; ++x) {
        for(int y=0; y<noph; ++y) {
            int i = x*noph+y;
            pt_ref[i][0] = x*steps+offsetw;
            pt_ref[i][1] = y*steps+offseth;
            p_init[i].setZero();
            pat.push_back(new PatClass<eInput,eOutput>(cpt,cpo,op,patchid));
            patchid++;
        }
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
ofdis::PatGridClass<eInput,eOutput>::~PatGridClass() {
    delete im_ao_eg;
    delete im_ao_dx_eg;
    delete im_ao_dy_eg;
    delete im_bo_eg;
    delete im_bo_dx_eg;
    delete im_bo_dy_eg;
    for(int i=0; i< nopatches; ++i)
        delete pat[i];
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::SetComplGrid(PatGridClass *cg_in) {
    cg = cg_in;
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::InitializeGrid(const float* im_ao_in, const float* im_ao_dx_in, const float* im_ao_dy_in) {
    im_ao = im_ao_in;
    im_ao_dx = im_ao_dx_in;
    im_ao_dy = im_ao_dy_in;
    new(im_ao_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao,cpt->height,cpt->width); // new placement operator
    new(im_ao_dx_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao_dx,cpt->height,cpt->width);
    new(im_ao_dy_eg) Eigen::Map<const Eigen::MatrixXf>(im_ao_dy,cpt->height,cpt->width);
#if USING_OPENMP
    #pragma omp parallel for schedule(static)
#endif //USING_OPENMP
    for (int i = 0; i < nopatches; ++i) {
        pat[i]->InitializePatch(im_ao_eg, im_ao_dx_eg, im_ao_dy_eg, pt_ref[i]);
        p_init[i].setZero();
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::SetTargetImage(const float* im_bo_in, const float* im_bo_dx_in, const float* im_bo_dy_in) {
    im_bo = im_bo_in;
    im_bo_dx = im_bo_dx_in;
    im_bo_dy = im_bo_dy_in;
    new(im_bo_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo,cpt->height,cpt->width); // new placement operator
    new(im_bo_dx_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo_dx,cpt->height,cpt->width); // new placement operator
    new(im_bo_dy_eg) Eigen::Map<const Eigen::MatrixXf>(im_bo_dy,cpt->height,cpt->width); // new placement operator
#if USING_OPENMP
    #pragma omp parallel for schedule(static)
#endif //USING_OPENMP
    for(int i = 0; i < nopatches; ++i)
        pat[i]->SetTargetImage(im_bo_eg, im_bo_dx_eg, im_bo_dy_eg);
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::Optimize() {
#if USING_OPENMP
    #pragma omp parallel for schedule(dynamic,10)
#endif //USING_OPENMP
    for(int i = 0; i < nopatches; ++i)
        pat[i]->OptimizeIter(p_init[i], true); // optimize until convergence
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::InitializeFromCoarserOF(const float* flow_prev) {
#if USING_OPENMP
    #pragma omp parallel for schedule(dynamic,10)
#endif //USING_OPENMP
    for (int ip = 0; ip < nopatches; ++ip) {
        int x = floor(pt_ref[ip][0] / 2); // better, but slower: use bil. interpolation here
        int y = floor(pt_ref[ip][1] / 2);
        int i = y*(cpt->width/2) + x;
        if(eOutput==ofdis::FlowOutput_OpticalFlow) {
            p_init[ip](0) = flow_prev[2*i]*2;
            p_init[ip](1) = flow_prev[2*i+1]*2;
        }
        else
            p_init[ip](0) = flow_prev[i]*2;
    }
}

template<ofdis::FlowInputType eInput, ofdis::FlowOutputType eOutput>
void ofdis::PatGridClass<eInput,eOutput>::AggregateFlowDense(float *flowout) const {
    float* we = new float[cpt->width * cpt->height];
    memset(flowout, 0, sizeof(float) * (op->nop * cpt->width * cpt->height) );
    memset(we,      0, sizeof(float) * (          cpt->width * cpt->height) );
#ifdef USE_PARALLEL_ON_FLOWAGGR // Using this enables OpenMP on flow aggregation. This can lead to race conditions. Experimentally we found that the result degrades only marginally. However, for our experiments we did not enable this.
    #pragma omp parallel for schedule(static)
#endif
    for(int ip = 0; ip < nopatches; ++ip) {
        if(pat[ip]->IsValid()) {
            const typename p_init_type::value_type* fl = pat[ip]->GetParam(); // flow/horiz displacement of this patch
            typename p_init_type::value_type flnew;
            const float* pweight = pat[ip]->GetpWeightPtr(); // use image error as weight
            int lb = -op->p_samp_s/2;
            int ub = op->p_samp_s/2-1;
            for(int y = lb; y <= ub; ++y) {
                for(int x = lb; x <= ub; ++x, ++pweight) {
                    int yt = (y + pt_ref[ip][1]);
                    int xt = (x + pt_ref[ip][0]);
                    if (xt >= 0 && yt >= 0 && xt < cpt->width && yt < cpt->height) {
                        int i = yt*cpt->width + xt;
                        float absw;
                        if(eInput==ofdis::FlowInput_RGB) {
                            absw = (float)(std::max(op->minerrval,*pweight)); ++pweight;
                            absw+= (float)(std::max(op->minerrval,*pweight)); ++pweight;
                            absw+= (float)(std::max(op->minerrval,*pweight));
                            absw = 1.0f/absw;
                        }
                        else
                            absw = 1.0f/(float)(std::max(op->minerrval,*pweight));
                        flnew = (*fl) * absw;
                        we[i] += absw;
                        if(eOutput==ofdis::FlowOutput_OpticalFlow) {
                            flowout[2*i]   += flnew[0];
                            flowout[2*i+1] += flnew[1];
                        }
                        else
                            flowout[i] += flnew[0];
                    }
                }
            }
        }
    }

    // if complementary (forward-backward merging) is given, integrate negative backward flow as well
    if(cg) {
        Eigen::Vector4f wbil; // bilinear weight vector
        Eigen::Vector4i pos;
#if USING_OPENMP
    #ifdef USE_PARALLEL_ON_FLOWAGGR
        #pragma omp parallel for schedule(static)
    #endif
#endif //USING_OPENMP
        for(int ip = 0; ip < cg->nopatches; ++ip) {
            if (cg->pat[ip]->IsValid()) {
                const typename p_init_type::value_type* fl = (cg->pat[ip]->GetParam()); // flow/horiz displacement of this patch
                typename p_init_type::value_type flnew;
                const Eigen::Vector2f rppos = cg->pat[ip]->GetPointPos(); // get patch position after optimization
                const float* pweight = cg->pat[ip]->GetpWeightPtr(); // use image error as weight
                Eigen::Vector2f resid;
                // compute bilinear weight vector
                pos[0] = ceil(rppos[0] +.00001); // make sure they are rounded up to natural number
                pos[1] = ceil(rppos[1] +.00001); // make sure they are rounded up to natural number
                pos[2] = floor(rppos[0]);
                pos[3] = floor(rppos[1]);
                resid[0] = rppos[0] - pos[2];
                resid[1] = rppos[1] - pos[3];
                wbil[0] = resid[0]*resid[1];
                wbil[1] = (1-resid[0])*resid[1];
                wbil[2] = resid[0]*(1-resid[1]);
                wbil[3] = (1-resid[0])*(1-resid[1]);
                int lb = -op->p_samp_s/2;
                int ub = op->p_samp_s/2-1;
                for(int y = lb; y <= ub; ++y) {
                    for(int x = lb; x <= ub; ++x, ++pweight) {
                        int yt = y + pos[1];
                        int xt = x + pos[0];
                        if(xt >= 1 && yt >= 1 && xt < (cpt->width-1) && yt < (cpt->height-1)) {
                            float absw;
                            if(eInput==ofdis::FlowInput_RGB) {
                                absw = (float)(std::max(op->minerrval,*pweight)); ++pweight;
                                absw+= (float)(std::max(op->minerrval,*pweight)); ++pweight;
                                absw+= (float)(std::max(op->minerrval,*pweight));
                                absw = 1.0f/absw;
                            }
                            else
                                absw = 1.0f/(float)(std::max(op->minerrval,*pweight));
                            flnew = (*fl) * absw;
                            int idxcc =  xt    +  yt   *cpt->width;
                            int idxfc = (xt-1) +  yt   *cpt->width;
                            int idxcf =  xt    + (yt-1)*cpt->width;
                            int idxff = (xt-1) + (yt-1)*cpt->width;
                            we[idxcc] += wbil[0] * absw;
                            we[idxfc] += wbil[1] * absw;
                            we[idxcf] += wbil[2] * absw;
                            we[idxff] += wbil[3] * absw;
                            if(eOutput==ofdis::FlowOutput_OpticalFlow) {
                                flowout[2*idxcc  ] -= wbil[0] * flnew[0];   // use reversed flow
                                flowout[2*idxcc+1] -= wbil[0] * flnew[1];
                                flowout[2*idxfc  ] -= wbil[1] * flnew[0];
                                flowout[2*idxfc+1] -= wbil[1] * flnew[1];
                                flowout[2*idxcf  ] -= wbil[2] * flnew[0];
                                flowout[2*idxcf+1] -= wbil[2] * flnew[1];
                                flowout[2*idxff  ] -= wbil[3] * flnew[0];
                                flowout[2*idxff+1] -= wbil[3] * flnew[1];
                            }
                            else {
                                flowout[idxcc] -= wbil[0] * flnew[0]; // simple averaging of inverse horizontal displacement
                                flowout[idxfc] -= wbil[1] * flnew[0];
                                flowout[idxcf] -= wbil[2] * flnew[0];
                                flowout[idxff] -= wbil[3] * flnew[0];
                            }
                        }
                    }
                }
            }
        }
    }
#if USING_OPENMP
    #pragma omp parallel for schedule(static, 100)
#endif //USING_OPENMP
    // normalize each pixel by dividing displacement by aggregated weights from all patches
    for (int yi = 0; yi < cpt->height; ++yi) {
        for (int xi = 0; xi < cpt->width; ++xi) {
            int i = yi*cpt->width + xi;
            if (we[i]>0) {
                if(eOutput==ofdis::FlowOutput_OpticalFlow) {
                    flowout[2*i  ] /= we[i];
                    flowout[2*i+1] /= we[i];
                }
                else
                    flowout[i] /= we[i];
            }
        }
    }
    delete[] we;
}

template class ofdis::PatGridClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::PatGridClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::PatGridClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_OpticalFlow>;
template class ofdis::PatGridClass<ofdis::FlowInput_Grayscale,ofdis::FlowOutput_StereoDepth>;
template class ofdis::PatGridClass<ofdis::FlowInput_Gradient,ofdis::FlowOutput_StereoDepth>;
template class ofdis::PatGridClass<ofdis::FlowInput_RGB,ofdis::FlowOutput_StereoDepth>;


#include <glog/logging.h>

#include <ros/ros.h>
#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>

#include <geometry_msgs/PoseStamped.h>
#include <pcl/filters/voxel_grid.h>

#include <tf2/LinearMath/Vector3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Transform.h>
#include <tf2/transform_datatypes.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/TransformStamped.h>

#include <Eigen/Geometry>

#include "Timer.h"
#include "ndt_algo.h"
#include "GPS_AHRS_sync.h"

NDTAlgo* pNDT;
tf2_ros::TransformBroadcaster* pTFbroadcaster;
bool ndt_result_visualization = false;
ros::Publisher pose_pub,gps_ahrs_pose_pub;
ros::Publisher aligned_cloud_pub;
void lidar_callback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
    ScopeTimer timer("callback_timer");
    if(pNDT->initializeNDTPoseGuess())//测试是否能获取gps-ahrs消息.
    {

        Eigen::Matrix4f gps_ahrs_initial_guess = pNDT->gps_ahrs_initial_guess;
        Eigen::Matrix3f rotmat = gps_ahrs_initial_guess.block(0,0,3,3);
        Eigen::Quaternionf quat(rotmat);
        geometry_msgs::PoseStamped gps_ahrs_pose_msg;

        gps_ahrs_pose_msg.header.frame_id="map";//地图坐标系的定位.
        gps_ahrs_pose_msg.header.stamp = cloud_msg->header.stamp;//与gps同步.

        auto& pos = gps_ahrs_pose_msg.pose.position;
        auto& orient = gps_ahrs_pose_msg.pose.orientation;
        orient.x = quat.x();
        orient.y = quat.y();
        orient.z = quat.z();
        orient.w = quat.w();

        pos.x = gps_ahrs_initial_guess(0,3);
        pos.y = gps_ahrs_initial_guess(1,3);
        pos.z = gps_ahrs_initial_guess(2,3);

        gps_ahrs_pose_pub.publish(gps_ahrs_pose_msg);
        LOG(INFO)<<"GPS AHRS pose published!"<<endl;

    }
    cout <<"callback"<<endl;
    LOG(INFO)<<"In lidar callback:"<<endl;
    LidarCloudT::Ptr pInputCloud(new LidarCloudT);
    pcl::fromROSMsg(*cloud_msg, *pInputCloud);
    LOG(INFO)<<"In lidar callback: lidar pointcloud size:"<<pInputCloud->size()<<endl;
    //Downsampling.
    LidarCloudT::Ptr downsampled(new LidarCloudT);
    pcl::VoxelGrid<LidarPointT> sor;
    sor.setInputCloud(pInputCloud);
    //sor.setLeafSize(0.2f, 0.2f, 0.2f);
    sor.setLeafSize(DOWNSAMPLE_SIZE, DOWNSAMPLE_SIZE, DOWNSAMPLE_SIZE);
    sor.filter(*downsampled);
    timer.watch("till cloud downsampled");
    LOG(INFO)<<"Downsampled pointcloud size:"<<downsampled->size()<<endl;
    NDTAlgo::RTMatrix4f output_ndt_pose;

    bool need_transformed_pointcloud = (ndt_result_visualization&&aligned_cloud_pub.getNumSubscribers()>0);
    LidarCloudT::Ptr transformed_cloud=nullptr;
    //bool ndt_success = pNDT->do_ndt_matching_without_initial_guess2(downsampled,output_ndt_pose,transformed_cloud,need_transformed_pointcloud);
    bool ndt_success = pNDT->doNDTMatching(downsampled,output_ndt_pose,transformed_cloud,need_transformed_pointcloud);

    if(ndt_success)
    {
        //publish output pose;
        Eigen::Matrix3f rotmat = output_ndt_pose.block(0,0,3,3);
        Eigen::Quaternionf quat(rotmat);
        geometry_msgs::PoseStamped ndt_pose_msg;

        ndt_pose_msg.header.frame_id="map";//地图坐标系的定位.
        ndt_pose_msg.header.stamp = cloud_msg->header.stamp;//与雷达同步.

        auto& pos = ndt_pose_msg.pose.position;
        auto& orient = ndt_pose_msg.pose.orientation;
        orient.x = quat.x();
        orient.y = quat.y();
        orient.z = quat.z();
        orient.w = quat.w();

        pos.x = output_ndt_pose(0,3);
        pos.y = output_ndt_pose(1,3);
        pos.z = output_ndt_pose(2,3);

        pose_pub.publish(ndt_pose_msg);

        //publish tf2 transfrom.
        NDTAlgo::RTMatrix4f lidar_to_map = output_ndt_pose.inverse();
        Eigen::Matrix3f l_m_rotmat = lidar_to_map.block(0,0,3,3);
        Eigen::Quaternionf l_m_quat(l_m_rotmat);

        geometry_msgs::TransformStamped map_lidar_trans_stamped;
        map_lidar_trans_stamped.header.frame_id = "lidar";// we've got lidar as the top node of tf tree.
        //So when localization is not avail, still we can solve lidar to body.
        map_lidar_trans_stamped.child_frame_id = "map";

        map_lidar_trans_stamped.header.stamp = cloud_msg->header.stamp;
        map_lidar_trans_stamped.transform.translation.x = lidar_to_map(0,3);
        map_lidar_trans_stamped.transform.translation.y = lidar_to_map(1,3);
        map_lidar_trans_stamped.transform.translation.z = lidar_to_map(2,3);
        map_lidar_trans_stamped.transform.rotation.x = l_m_quat.x();
        map_lidar_trans_stamped.transform.rotation.y = l_m_quat.y();
        map_lidar_trans_stamped.transform.rotation.z = l_m_quat.z();
        map_lidar_trans_stamped.transform.rotation.w = l_m_quat.w();

        pTFbroadcaster->sendTransform(map_lidar_trans_stamped);

        if(need_transformed_pointcloud&&transformed_cloud!=nullptr)
        {
            sensor_msgs::PointCloud2 transformed_cloud_msg;
            pcl::toROSMsg(*transformed_cloud,transformed_cloud_msg);
            transformed_cloud_msg.header.frame_id = "map";
            transformed_cloud_msg.header.stamp = cloud_msg->header.stamp;
            aligned_cloud_pub.publish(transformed_cloud_msg);
        }
        //publish merged pointcloud
    }
    else
    {
        LOG(WARNING)<<"NDT matching failed!"<<endl;
    }
}

GPS_AHRS_Synchronizer gps_ahrs_sync;

void gps_ahrs_callback(const sensor_msgs::NavSatFixConstPtr& gps_msg, const nav_msgs::OdometryConstPtr& odom)// 同步gps和ahrs.
{
    LOG(INFO)<<"In gps_ahrs_callback"<<endl;
    gps_ahrs_sync.sync_mutex.lock();
    gps_ahrs_sync.gps_msg = *gps_msg;
    gps_ahrs_sync.ahrs_msg = *odom;
    gps_ahrs_sync.ever_init = true;
    gps_ahrs_sync.sync_mutex.unlock();
};

int main(int argc,char** argv)
{
    FLAGS_alsologtostderr = 1;
    google::InitGoogleLogging("ndt_matching_node");
    ros::init(argc,argv,"ndt_matching_node");
    ros::NodeHandle nh;
    NDTAlgo ndt(&gps_ahrs_sync);
    ndt.loadPCDMap();
    pNDT=&ndt;
    tf2_ros::TransformBroadcaster br;
    pTFbroadcaster = &br;

    string lidar_topic_name;
    ros::param::get("lidar_topic_name",lidar_topic_name);
    ros::param::get("ndt_result_visualization",ndt_result_visualization);
    LOG(INFO)<<"Subscribing lidar topic name:"<<lidar_topic_name<<endl;
    pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/gaas/localization/registration_pose",1);
    gps_ahrs_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/gaas/localization/original_gps_ahrs_pose",1);
    if(ndt_result_visualization)
    {
        aligned_cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/gaas/visualization/localization/ndt_merged_cloud",1);
    }
    ros::Subscriber lidar_sub = nh.subscribe("/gaas/preprocessing/velodyne_downsampled",1,lidar_callback);

    message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub(nh, "/mavros/global_position/raw/fix", 1);
    //message_filters::Subscriber<sensor_msgs::NavSatFix> gps_sub(nh, "/mavros/global_position/global", 1);
    //message_filters::Subscriber<nav_msgs::Odometry> ahrs_sub(nh, "/mavros/local_position/odom", 1);
    message_filters::Subscriber<nav_msgs::Odometry> ahrs_sub(nh, "/mavros/global_position/local", 1);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::NavSatFix, nav_msgs::Odometry> MySyncPolicy;
    // ApproximateTime takes a queue size as its constructor argument, hence MySyncPolicy(10)
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(3), gps_sub, ahrs_sub);
    sync.registerCallback(boost::bind(&gps_ahrs_callback, _1, _2));
    while(ros::ok())
    {
        ros::spinOnce();
    }
    return 0;
}


#pragma once

#include "trajectory.hpp"
#include <Eigen/Dense>

namespace hebi {

class Leg;

// Represents a single step being actively taken by a leg.
class Step
{
public:
  Step(double start_time, Leg* leg); // NOTE: I don't like this circular dependency on Leg here...
  // TODO: don't call update from constructor?

  // Note: returns 'true' if complete
  bool update(double t, Leg* leg); // NOTE: I don't like this circular dependency on Leg here...

  const Eigen::Vector3d& getTouchDown() const { return touch_down_; }
  void computeState(double t, Eigen::VectorXd& angles, Eigen::VectorXd& vels, Eigen::VectorXd& accels) const;

  static constexpr float period_ = 0.7f; // seconds
  static constexpr float overshoot_ = 0.3f; // factor of step to overshoot
  static constexpr float height_ = 0.04f; // in meters
  // When creating trajectories, don't use waypoints that are too close together
  static constexpr float ignore_waypoint_threshold_ = 0.01; // 10 ms (in seconds)
  double getStartTime() const { return start_time_; }
private:
  // TODO: read from XML -- the phase points of the leg
  static const int num_phase_pts_ = 3;
  const double phase_[num_phase_pts_] = {0, 0.5, 1};
  double start_time_;
  // Time to hit each waypoint
  std::vector<double> time_;

  Eigen::Vector3d lift_off_vel_;

  // Step waypoints: lift up -> high point -> touch down
  Eigen::Vector3d lift_up_;
  Eigen::Vector3d mid_step_1_;
  Eigen::Vector3d mid_step_2_;
  Eigen::Vector3d touch_down_;

  std::shared_ptr<trajectory::Trajectory> trajectory_;

  // Allow Eigen member variables:
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace hebi


#define BOOST_TEST_MODULE tracer
#include <boost/test/included/unit_test.hpp>

#include "../src/random.hpp"
#include "../src/tracer.hpp"

using namespace std;

BOOST_AUTO_TEST_CASE(test_correct_create_orthonormal_basis) {
    Vec w(1.0, 0.0, 0.0);
    auto[u, v] = create_orthonormal_basis(w);

    double result = dot(w, u);
    BOOST_CHECK_EQUAL(result, 0.0);

    result = dot(w, v);
    BOOST_CHECK_EQUAL(result, 0.0);
}

BOOST_AUTO_TEST_CASE(test_correct_sample_from_hemisphere) {
    Vec w(1.0, 0.0, 0.0);
    auto[u, v] = create_orthonormal_basis(w);

    UniformRealGenerator rnd;

    Vec result = sample_from_hemisphere(u, v, w, rnd, 1.0);

    BOOST_CHECK(result.x >= -1.0);
    BOOST_CHECK(result.x <= 1.0);
    BOOST_CHECK(result.y >= -1.0);
    BOOST_CHECK(result.y <= 1.0);
    BOOST_CHECK(result.z >= -1.0);
    BOOST_CHECK(result.z <= 1.0);
}


//| Copyright Inria May 2015
//| This project has received funding from the European Research Council (ERC) under
//| the European Union's Horizon 2020 research and innovation programme (grant
//| agreement No 637972) - see http://www.resibots.eu
//|
//| Contributor(s):
//|   - Jean-Baptiste Mouret (jean-baptiste.mouret@inria.fr)
//|   - Antoine Cully (antoinecully@gmail.com)
//|   - Konstantinos Chatzilygeroudis (konstantinos.chatzilygeroudis@inria.fr)
//|   - Federico Allocati (fede.allocati@gmail.com)
//|   - Vaios Papaspyros (b.papaspyros@gmail.com)
//|   - Roberto Rama (bertoski@gmail.com)
//|
//| This software is a computer library whose purpose is to optimize continuous,
//| black-box functions. It mainly implements Gaussian processes and Bayesian
//| optimization.
//| Main repository: http://github.com/resibots/limbo
//| Documentation: http://www.resibots.eu/limbo
//|
//| This software is governed by the CeCILL-C license under French law and
//| abiding by the rules of distribution of free software.  You can  use,
//| modify and/ or redistribute the software under the terms of the CeCILL-C
//| license as circulated by CEA, CNRS and INRIA at the following URL
//| "http://www.cecill.info".
//|
//| As a counterpart to the access to the source code and  rights to copy,
//| modify and redistribute granted by the license, users are provided only
//| with a limited warranty  and the software's author,  the holder of the
//| economic rights,  and the successive licensors  have only  limited
//| liability.
//|
//| In this respect, the user's attention is drawn to the risks associated
//| with loading,  using,  modifying and/or developing or reproducing the
//| software by the user in light of its specific status of free software,
//| that may mean  that it is complicated to manipulate,  and  that  also
//| therefore means  that it is reserved for developers  and  experienced
//| professionals having in-depth computer knowledge. Users are therefore
//| encouraged to load and test the software's suitability as regards their
//| requirements in conditions enabling the security of their systems and/or
//| data to be ensured and,  more generally, to use and operate it in the
//| same conditions as regards security.
//|
//| The fact that you are presently reading this means that you have had
//| knowledge of the CeCILL-C license and that you accept its terms.
//|
#ifndef LIMBO_INIT_GRID_SAMPLING_HPP
#define LIMBO_INIT_GRID_SAMPLING_HPP

#include <Eigen/Core>

#include <limbo/tools/macros.hpp>

namespace limbo {
    namespace defaults {
        struct init_gridsampling {
            ///@ingroup init_defaults
            BO_PARAM(int, bins, 5);
        };
    }
    namespace init {
        /** @ingroup init
          \rst
          Grid sampling.

          Parameter:
            - ``int bins`` (number of bins)
          \endrst
        */
        template <typename Params>
        struct GridSampling {
            template <typename StateFunction, typename AggregatorFunction, typename Opt>
            void operator()(const StateFunction& seval, const AggregatorFunction&, Opt& opt) const
            {
                _explore(0, seval, Eigen::VectorXd::Constant(StateFunction::dim_in(), 0), opt);
            }

        private:
            // recursively explore all the dimensions
            template <typename StateFunction, typename Opt>
            void _explore(int dim_in, const StateFunction& seval, const Eigen::VectorXd& current,
                Opt& opt) const
            {
                for (double x = 0; x <= 1.0f; x += 1.0f / (double)Params::init_gridsampling::bins()) {
                    Eigen::VectorXd point = current;
                    point[dim_in] = x;
                    if (dim_in == current.size() - 1) {
                        opt.eval_and_add(seval, point);
                    }
                    else {
                        _explore(dim_in + 1, seval, point, opt);
                    }
                }
            }
        };
    }
}
#endif


//
// Copyright © 2017 Arm Ltd. All rights reserved.
// SPDX-License-Identifier: MIT
//

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersSerializeFixture.hpp"
#include "../Deserializer.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(DeserializeParser)

struct ConstantAddFixture : public ParserFlatbuffersSerializeFixture
{
    explicit ConstantAddFixture(const std::string & shape,
                                const std::string & constTensorDatatype,
                                const std::string & constData,
                                const std::string & dataType)
    {
        m_JsonString = R"(
        {
                inputIds: [0],
                outputIds: [3],
                layers: [
                {
                    layer_type: "InputLayer",
                    layer: {
                          base: {
                                layerBindingId: 0,
                                base: {
                                    index: 0,
                                    layerName: "InputLayer1",
                                    layerType: "Input",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [ {
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + shape + R"(,
                                            dataType: )" + dataType + R"(
                                        },
                                    }],
                                 },}},
                },
                {
                layer_type: "ConstantLayer",
                layer: {
                            base: {
                                  index:1,
                                  layerName: "ConstantLayer",
                                  layerType: "Constant",
                                  outputSlots: [ {
                                      index: 0,
                                      tensorInfo: {
                                          dimensions: )" + shape + R"(,
                                          dataType: )" + dataType + R"(,
                                      },
                                  }],
                                  inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                    }],
                                },
                            input: {
                                info: {
                                         dimensions: )" + shape + R"(,
                                         dataType: )" + dataType + R"(
                                     },
                                data_type: )" + constTensorDatatype + R"(,
                                data: {
                                    data: )" + constData + R"(,
                                    } }
                        },
                },
                {
                layer_type: "AdditionLayer",
                layer : {
                        base: {
                             index:2,
                             layerName: "AdditionLayer",
                             layerType: "Addition",
                             inputSlots: [
                                            {
                                             index: 0,
                                             connection: {sourceLayerIndex:0, outputSlotIndex:0 },
                                            },
                                            {
                                             index: 1,
                                             connection: {sourceLayerIndex:1, outputSlotIndex:0 },
                                            }
                             ],
                             outputSlots: [ {
                                 index: 0,
                                 tensorInfo: {
                                     dimensions: )" + shape + R"(,
                                     dataType: )" + dataType + R"(
                                 },
                             }],
                            }},
                },
                {
                layer_type: "OutputLayer",
                layer: {
                        base:{
                              layerBindingId: 0,
                              base: {
                                    index: 3,
                                    layerName: "OutputLayer",
                                    layerType: "Output",
                                    inputSlots: [{
                                        index: 0,
                                        connection: {sourceLayerIndex:2, outputSlotIndex:0 },
                                    }],
                                    outputSlots: [ {
                                        index: 0,
                                        tensorInfo: {
                                            dimensions: )" + shape + R"(,
                                            dataType: )" + dataType + R"(
                                        },
                                }],
                            }}},
                }]
         }
        )";
        SetupSingleInputSingleOutput("InputLayer1", "OutputLayer");
    }
};

struct SimpleConstantAddFixture : ConstantAddFixture
{
    SimpleConstantAddFixture()
            : ConstantAddFixture("[ 2, 3 ]",             // shape
                                 "ByteData",             // constDataType
                                 "[ 1, 2, 3, 4, 5, 6 ]", // constData
                                 "QuantisedAsymm8")      // datatype

    {}
};

BOOST_FIXTURE_TEST_CASE(SimpleConstantAddQuantisedAsymm8, SimpleConstantAddFixture)
{
    RunTest<2, armnn::DataType::QAsymmU8>(
            0,
            { 1, 2, 3, 4, 5, 6  },
            { 2, 4, 6, 8, 10, 12 });
}

BOOST_AUTO_TEST_SUITE_END()


//  (C) Copyright Eric Niebler 2005.
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <boost/test/unit_test.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/numeric/integrate.hpp>
#include <boost/time_series/ordered_inserter.hpp>

using namespace boost;
using namespace unit_test;

///////////////////////////////////////////////////////////////////////////////
// test_integrate
//
void test_integrate()
{
    using namespace time_series;

    piecewise_constant_series<int> p1(discretization = 30);
    make_ordered_inserter(p1)
        (2, 0, 10)
        (4, 10, 20)
        (2, 90, 99)
    .commit();
    BOOST_CHECK_EQUAL(2340, integrate(p1));

    piecewise_constant_series<int, monthly> p2;
    make_ordered_inserter(p2)
        (2, 0, 10)
        (4, 10, 20)
        (2, 90, 99)
    .commit();
    BOOST_CHECK_EQUAL(2340, integrate(p2));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("integrate test");

    test->add(BOOST_TEST_CASE(&test_integrate));

    return test;
}