// Boost random_generator.hpp header file  ----------------------------------------------//

// Copyright 2010 Andy Tompkins.
// 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_UUID_RANDOM_GENERATOR_HPP
#define BOOST_UUID_RANDOM_GENERATOR_HPP

#include <boost/uuid/uuid.hpp>
#include <boost/uuid/seed_rng.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/assert.hpp>
#include <boost/shared_ptr.hpp>
#include <limits>

namespace boost {
namespace uuids {

// generate a random-based uuid
template <typename UniformRandomNumberGenerator>
class basic_random_generator {
private:
    typedef uniform_int<unsigned long> distribution_type;
    typedef variate_generator<UniformRandomNumberGenerator*, distribution_type> generator_type;

    struct null_deleter
    {
        void operator()(void const *) const {}
    };

public:
    typedef uuid result_type;

    // default constructor creates the random number generator
    basic_random_generator()
        : pURNG(new UniformRandomNumberGenerator)
        , generator
          ( pURNG.get()
          , distribution_type
            ( (std::numeric_limits<unsigned long>::min)()
            , (std::numeric_limits<unsigned long>::max)()
            )
          )
    {
        // seed the random number generator
        detail::seed(*pURNG);
    }

    // keep a reference to a random number generator
    // don't seed a given random number generator
    explicit basic_random_generator(UniformRandomNumberGenerator& gen)
        : pURNG(&gen, null_deleter())
        , generator
          ( pURNG.get()
          , distribution_type
            ( (std::numeric_limits<unsigned long>::min)()
            , (std::numeric_limits<unsigned long>::max)()
            )
          )
    {}

    // keep a pointer to a random number generator
    // don't seed a given random number generator
    explicit basic_random_generator(UniformRandomNumberGenerator* pGen)
        : pURNG(pGen, null_deleter())
        , generator
          ( pURNG.get()
          , distribution_type
            ( (std::numeric_limits<unsigned long>::min)()
            , (std::numeric_limits<unsigned long>::max)()
            )
          )
    {
        BOOST_ASSERT(pURNG);
    }

    uuid operator()()
    {
        uuid u;

        int i=0;
        unsigned long random_value = generator();
        for (uuid::iterator it=u.begin(); it!=u.end(); ++it, ++i) {
            if (i==sizeof(unsigned long)) {
                random_value = generator();
                i = 0;
            }

			// static_cast gets rid of warnings of converting unsigned long to boost::uint8_t
            *it = static_cast<uuid::value_type>((random_value >> (i*8)) & 0xFF);
        }

        // set variant
        // must be 0b10xxxxxx
        *(u.begin()+8) &= 0xBF;
        *(u.begin()+8) |= 0x80;

        // set version
        // must be 0b0100xxxx
        *(u.begin()+6) &= 0x4F; //0b01001111
        *(u.begin()+6) |= 0x40; //0b01000000

        return u;
    }

private:
    shared_ptr<UniformRandomNumberGenerator> pURNG;
    generator_type generator;
};

typedef basic_random_generator<mt19937> random_generator;

}} // namespace boost::uuids

#endif //BOOST_UUID_RANDOM_GENERATOR_HPP


/*=========================================================================

  Program:   Visualization Toolkit
  Module:    vtkBoostLogWeighting.cxx

-------------------------------------------------------------------------
  Copyright 2008 Sandia Corporation.
  Under the terms of Contract DE-AC04-94AL85000 with Sandia Corporation,
  the U.S. Government retains certain rights in this software.
-------------------------------------------------------------------------

  Copyright (c) Ken Martin, Will Schroeder, Bill Lorensen
  All rights reserved.
  See Copyright.txt or http://www.kitware.com/Copyright.htm for details.

     This software is distributed WITHOUT ANY WARRANTY; without even
     the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
     PURPOSE.  See the above copyright notice for more information.

=========================================================================*/

#include "vtkArrayCoordinates.h"
#include "vtkCommand.h"
#include "vtkInformation.h"
#include "vtkInformationVector.h"
#include "vtkBoostLogWeighting.h"
#include "vtkObjectFactory.h"
#include "vtkSmartPointer.h"
#include "vtkTypedArray.h"

#include <boost/math/special_functions/log1p.hpp>
#include <boost/version.hpp>
#if BOOST_VERSION < 103400
  #error "vtkBoostLogWeighting requires Boost 1.34.0 or later"
#endif

///////////////////////////////////////////////////////////////////////////////
// vtkBoostLogWeighting

vtkCxxRevisionMacro(vtkBoostLogWeighting, "1.1");
vtkStandardNewMacro(vtkBoostLogWeighting);

vtkBoostLogWeighting::vtkBoostLogWeighting()
{
}

vtkBoostLogWeighting::~vtkBoostLogWeighting()
{
}

void vtkBoostLogWeighting::PrintSelf(ostream& os, vtkIndent indent)
{
  this->Superclass::PrintSelf(os, indent);
}

int vtkBoostLogWeighting::RequestData(
  vtkInformation*,
  vtkInformationVector** inputVector,
  vtkInformationVector* outputVector)
{
  vtkArrayData* const input = vtkArrayData::GetData(inputVector[0]);
  vtkArrayData* const output = vtkArrayData::GetData(outputVector);

  if(vtkTypedArray<double>* const input_array = vtkTypedArray<double>::SafeDownCast(input->GetArray()))
    {
    vtkTypedArray<double>* const output_array = vtkTypedArray<double>::SafeDownCast(input_array->DeepCopy());
    output->SetArray(output_array);
    output_array->Delete();

    const vtkIdType value_count = input_array->GetNonNullSize();

    for(vtkIdType i = 0; i != value_count; ++i)
      {
      output_array->SetValueN(i, boost::math::log1p(output_array->GetValueN(i)));

      double progress = static_cast<double>(i) / static_cast<double>(value_count);
      this->InvokeEvent(vtkCommand::ProgressEvent, &progress);
      }
    }
  else
    {
    vtkErrorMacro(<< "Unsupported input array type");
    return 0;
    }

  return 1;
}


#include "SelfPower.hh"
#include "TypesFunctions.hh"
#include <Eigen/Core>
#include <cmath>

#ifdef GNA_CUDA_SUPPORT
#include "cuElementary.hh"
#include "GpuBasics.hh"
#include "DataLocation.hh"
#endif

/**
 * @brief Constructor.
 *
 * Creates two transformations for functions with positive and negative power.
 *
 * @param scalename="sp_scale" -- name of a variable to scale the argument.
 */
SelfPower::SelfPower(const char* scalename/*="sp_scale"*/) {
    variable_(&m_scale, scalename);

    transformation_("selfpower")
        .input("points")
	.output("result")
	.types(TypesFunctions::ifPoints<0>, TypesFunctions::pass<0>)
	.func(&SelfPower::calculate)
#ifdef GNA_CUDA_SUPPORT
	.func("gpu", &SelfPower::gpu_calculate, DataLocation::Device)
#endif
      ;

    transformation_("selfpower_inv")
        .input("points")
	.output("result")
	.types(TypesFunctions::ifPoints<0>, TypesFunctions::pass<0>)
	.func(&SelfPower::calculate_inv)
      ;
}

#ifdef GNA_CUDA_SUPPORT
void SelfPower::gpu_calculate(FunctionArgs& fargs) {
    fargs.args.touch();
    auto& gpuargs=fargs.gpu;
    gpuargs->provideSignatureDevice();
    cuselfpower(gpuargs->args, gpuargs->rets, fargs.args[0].arr.size(), gpuargs->nargs,m_scale.value());
}
#endif

/**
 * @brief Calculate the value of function with positive power.
 */
void SelfPower::calculate(FunctionArgs& fargs){
    auto& res = fargs.rets[0].x = fargs.args[0].x/m_scale.value();
    res=res.pow(res);
}

/**
 * @brief Calculate the value of function with negative power.
 */
void SelfPower::calculate_inv(FunctionArgs& fargs){
    auto& res = fargs.rets[0].x = fargs.args[0].x/m_scale.value();
    res=res.pow(-res);
}


#pragma once

// Armadillo
#include <armadillo>

// Mantella
#include "mantella_bits/optimisationProblem/kinematicallyRedundantMachines.hpp"

namespace mant {
  namespace krm {
    class ParallelKinematicMachine6PRUS : public KinematicallyRedundantMachines {
     public:
      explicit ParallelKinematicMachine6PRUS();

     protected:
      const arma::mat::fixed<3, 6> redundantJointsPosition_;
      const arma::mat::fixed<3, 6> redundantJointsAngles_;
      const arma::mat::fixed<3, 6> baseJointsNormal_;
      const arma::mat::fixed<2, 6> linkLengths_;
      const arma::mat::fixed<3, 6> endEffectorJointsRelativePosition_;
    };
  }
}


#include <boost/graph/random_spanning_tree.hpp>


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

#include <boost/simd/function/simd/interleave_first.hpp>
#include <boost/simd/function/simd/interleave_second.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;

  BOOST_DISPATCH_OVERLOAD ( split_multiplies_
                          , (typename A0, typename A1)
                          , bs::sse4_1_
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bs::pack_<bd::int32_<A0>, bs::sse_>
                          , bs::pack_<bd::int64_<A1>, bs::sse_>
                          , bs::pack_<bd::int64_<A1>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE void operator() ( const A0 & a0
                                      , const A0 & a1
                                      , A1 & a2
                                      , A1 & a3) const BOOST_NOEXCEPT
    {
      A1 lo = _mm_mul_epi32(a0, a1);
      A1 hi = _mm_mul_epi32(_mm_srli_si128(a0, 4), _mm_srli_si128(a1, 4));

      a2 = interleave_first(lo, hi);
      a3 = interleave_second(lo, hi);
    }
  };
} } }

#endif


#ifndef SKYLARK_BLOCKADMM_HPP
#define SKYLARK_BLOCKADMM_HPP

#include <elemental.hpp>
#include <skylark.hpp>
#include <cmath>
#include <boost/mpi.hpp>

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

#include "../utility/timer.hpp"
#include "hilbert.hpp"

// Columns are examples, rows are features
typedef elem::DistMatrix<double, elem::STAR, elem::VC> DistInputMatrixType;

// Rows are examples, columns are target values
typedef elem::DistMatrix<double, elem::VC, elem::STAR> DistTargetMatrixType;

typedef elem::Matrix<double> LocalMatrixType;
typedef skylark::base::sparse_matrix_t<double> sparse_matrix_t;

template <class T>
class BlockADMMSolver
{
public:

    typedef skylark::sketch::sketch_transform_t<T, LocalMatrixType>
    feature_transform_t;
    typedef std::vector<const feature_transform_t *> feature_transform_array_t;

    // No feature transdeforms (aka just linear regression).
    BlockADMMSolver(const lossfunction* loss,
        const regularization* regularizer,
        double lambda, // regularization parameter
        int NumFeatures,
        int NumFeaturePartitions = 1);

    // Easy interface, aka kernel based.
    template<typename Kernel, typename MapTypeTag>
    BlockADMMSolver<T>(skylark::base::context_t& context,
        const lossfunction* loss,
        const regularization* regularizer,
        double lambda, // regularization parameter
        int NumFeatures,
        Kernel kernel,
        MapTypeTag tag,
        int NumFeaturePartitions = 1);

    // Easy interface, aka kernel based, with quasi-random features.
    template<typename Kernel>
    BlockADMMSolver<T>(skylark::base::context_t& context,
        const lossfunction* loss,
        const regularization* regularizer,
        double lambda, // regularization parameter
        int NumFeatures,
        Kernel kernel,
        skylark::ml::quasi_feature_transform_tag tag,
        int NumFeaturePartitions);

    // Guru interface.
    BlockADMMSolver<T>(const lossfunction* loss,
        const regularization* regularizer,
        const feature_transform_array_t& featureMaps,
        double lambda, // regularization parameter
        bool ScaleFeatureMaps = true);

    void set_nthreads(int NumThreads) { this->NumThreads = NumThreads; }
    void set_rho(double RHO) { this->RHO = RHO; }
    void set_maxiter(double MAXITER) { this->MAXITER = MAXITER; }
    void set_tol(double TOL) { this->TOL = TOL; }
    void set_cache_transform(bool CacheTransforms) {this->CacheTransforms = CacheTransforms;}

    ~BlockADMMSolver();

    void InitializeFactorizationCache();
    void InitializeTransformCache(int n);

    skylark::ml::model_t<T, LocalMatrixType>* train(T& X,
        LocalMatrixType& Y, T& Xv, LocalMatrixType& Yv,
        const boost::mpi::communicator& comm);

    int get_numfeatures() {return NumFeatures;}

    feature_transform_array_t& get_feature_maps() {return featureMaps;}

private:

    feature_transform_array_t featureMaps;
    int NumFeatures;
    int NumFeaturePartitions;
    lossfunction* loss;
    regularization* regularizer;
    std::vector<int> starts, finishes;
    bool ScaleFeatureMaps;
    bool OwnFeatureMaps;
    LocalMatrixType **Cache;
    LocalMatrixType **TransformCache;
    int NumThreads;

    double lambda;
    double RHO;
    int MAXITER;
    double TOL;

    bool CacheTransforms;
};

template <class T>
void BlockADMMSolver<T>::InitializeFactorizationCache() {
    Cache = new LocalMatrixType* [NumFeaturePartitions];
    for(int j=0; j<NumFeaturePartitions; j++) {
        int start = starts[j];
        int finish = finishes[j];
        int sj = finish - start  + 1;
        Cache[j]  = new elem::Matrix<double>(sj, sj);
    }
}

template <class T>
void BlockADMMSolver<T>::InitializeTransformCache(int n) {
    TransformCache = new LocalMatrixType* [NumFeaturePartitions];
    for(int j=0; j<NumFeaturePartitions; j++) {
        int start = starts[j];
        int finish = finishes[j];
        int sj = finish - start  + 1;
        TransformCache[j]  = new elem::Matrix<double>(sj, n);
    }
}

// No feature transforms (aka just linear regression).
template <class T>
BlockADMMSolver<T>::BlockADMMSolver(
        const lossfunction* loss,
        const regularization* regularizer,
        double lambda, // regularization parameter
        int NumFeatures,
        int NumFeaturePartitions) :
        NumFeatures(NumFeatures),
            NumFeaturePartitions(NumFeaturePartitions),
            starts(NumFeaturePartitions), finishes(NumFeaturePartitions),
            NumThreads(1), RHO(1.0), MAXITER(1000), TOL(0.1) {

    this->loss = const_cast<lossfunction *> (loss);
    this->regularizer = const_cast<regularization *> (regularizer);
    this->lambda = lambda;
    this->NumFeaturePartitions = NumFeaturePartitions;
    int blksize = int(ceil(double(NumFeatures) / NumFeaturePartitions));
    for(int i = 0; i < NumFeaturePartitions; i++) {
        starts[i] = i * blksize;
        finishes[i] = std::min((i + 1) * blksize, NumFeatures) - 1;
    }
    this->ScaleFeatureMaps = false;
    OwnFeatureMaps = false;
    InitializeFactorizationCache();
    CacheTransforms = false;
}

// Easy interface, aka kernel based.
template<class T>
template<typename Kernel, typename MapTypeTag>
BlockADMMSolver<T>::BlockADMMSolver(skylark::base::context_t& context,
    const lossfunction* loss,
    const regularization* regularizer,
    double lambda, // regularization parameter
    int NumFeatures,
    Kernel kernel,
    MapTypeTag tag,
    int NumFeaturePartitions) :
    featureMaps(NumFeaturePartitions),
    NumFeatures(NumFeatures), NumFeaturePartitions(NumFeaturePartitions),
    starts(NumFeaturePartitions), finishes(NumFeaturePartitions),
    NumThreads(1), RHO(1.0), MAXITER(1000), TOL(0.1) {

    this->loss = const_cast<lossfunction *> (loss);
    this->regularizer = const_cast<regularization *> (regularizer);
    this->lambda = lambda;
    int blksize = int(ceil(double(NumFeatures) / NumFeaturePartitions));
    for(int i = 0; i < NumFeaturePartitions; i++) {
        starts[i] = i * blksize;
        finishes[i] = std::min((i + 1) * blksize, NumFeatures) - 1;
        int sj = finishes[i] - starts[i] + 1;
        featureMaps[i] =
            kernel.template create_rft< T, LocalMatrixType >(sj, tag, context);
    }
    this->ScaleFeatureMaps = true;
    OwnFeatureMaps = true;
    InitializeFactorizationCache();
    CacheTransforms = false;
}

// Easy interface, aka kernel based, with quasi-random features.
template<class T>
template<typename Kernel>
BlockADMMSolver<T>::BlockADMMSolver(skylark::base::context_t& context,
    const lossfunction* loss,
    const regularization* regularizer,
    double lambda, // regularization parameter
    int NumFeatures,
    Kernel kernel,
    skylark::ml::quasi_feature_transform_tag tag,
    int NumFeaturePartitions) :
    featureMaps(NumFeaturePartitions),
    NumFeatures(NumFeatures), NumFeaturePartitions(NumFeaturePartitions),
    starts(NumFeaturePartitions), finishes(NumFeaturePartitions),
    NumThreads(1), RHO(1.0), MAXITER(1000), TOL(0.1) {

    this->loss = const_cast<lossfunction *> (loss);
    this->regularizer = const_cast<regularization *> (regularizer);
    this->lambda = lambda;
    int blksize = int(ceil(double(NumFeatures) / NumFeaturePartitions));
    skylark::utility::leaped_halton_sequence_t<double>
        qmcseq(kernel.qrft_sequence_dim()); // TODO size
    for(int i = 0; i < NumFeaturePartitions; i++) {
        starts[i] = i * blksize;
        finishes[i] = std::min((i + 1) * blksize, NumFeatures) - 1;
        int sj = finishes[i] - starts[i] + 1;
        featureMaps[i] =
            kernel.template create_qrft< T, LocalMatrixType,
              skylark::utility::leaped_halton_sequence_t>(sj, qmcseq,
                  starts[i], context);
    }
    this->ScaleFeatureMaps = true;
    OwnFeatureMaps = true;
    InitializeFactorizationCache();
    CacheTransforms = false;
}

// Guru interface
template <class T>
BlockADMMSolver<T>::BlockADMMSolver(const lossfunction* loss,
    const regularization* regularizer,
    const feature_transform_array_t &featureMaps,
    double lambda,
    bool ScaleFeatureMaps) :
    featureMaps(featureMaps),
    NumFeaturePartitions(featureMaps.size()),
    starts(NumFeaturePartitions), finishes(NumFeaturePartitions),
    NumThreads(1), RHO(1.0), MAXITER(1000), TOL(0.1)  {

    this->loss = const_cast<lossfunction *> (loss);
    this->regularizer = const_cast<regularization *> (regularizer);
    this->lambda = lambda;
    NumFeaturePartitions = featureMaps.size();
    NumFeatures = 0;
    for(int i = 0; i < NumFeaturePartitions; i++) {
        starts[i] = NumFeatures;
        finishes[i] = NumFeatures + featureMaps[i]->get_S() - 1;
        NumFeatures += featureMaps[i]->get_S();
    }
    this->ScaleFeatureMaps = ScaleFeatureMaps;
    OwnFeatureMaps = false;
    InitializeFactorizationCache();
    CacheTransforms = false;
}

template <class T>
BlockADMMSolver<T>::~BlockADMMSolver() {
    for(int i=0; i  < NumFeaturePartitions; i++) {
        delete Cache[i];
        if (OwnFeatureMaps)
            delete featureMaps[i];
    }
    delete[] Cache;
}

template <class T>
skylark::ml::model_t<T, LocalMatrixType>* BlockADMMSolver<T>::train(T& X, LocalMatrixType& Y, T& Xv, LocalMatrixType& Yv,
    const boost::mpi::communicator& comm) {

       int rank = comm.rank();
       int size = comm.size();

       int P = size;

       int ni = skylark::base::Width(X);
       int d = skylark::base::Height(X);
       int targets = GetNumTargets(comm, Y);

       skylark::ml::model_t<T, LocalMatrixType>* model =
           new skylark::ml::model_t<T, LocalMatrixType>(featureMaps,
               ScaleFeatureMaps, NumFeatures, targets);

       elem::Matrix<double> Wbar;
       elem::View(Wbar, model->get_coef());

       int k = Wbar.Width();

       // number of classes, targets - to generalize

       int D = NumFeatures;

       // exception: check if D = Wbar.Height();

       LocalMatrixType O(k, ni); //uses default Grid
       elem::MakeZeros(O);

       LocalMatrixType Obar(k, ni); //uses default Grid
       elem::MakeZeros(Obar);

       LocalMatrixType nu(k, ni); //uses default Grid
       elem::MakeZeros(nu);

       LocalMatrixType W, mu, Wi, mu_ij, ZtObar_ij;

       if(rank==0) {
           elem::Zeros(W,  D, k);
           elem::Zeros(mu, D, k);
       }
       elem::Zeros(Wi, D, k);
       elem::Zeros(mu_ij, D, k);
       elem::Zeros(ZtObar_ij, D, k);

       int iter = 0;

       // int ni = O.LocalWidth();

       //elem::Matrix<double> x = X.Matrix();
       //elem::Matrix<double> y = Y.Matrix();

       double localloss = loss->evaluate(O, Y);
       double totalloss, accuracy, obj;

       int Dk = D*k;
       int nik  = ni*k;
       int start, finish, sj;

       boost::mpi::timer timer;

       LocalMatrixType sum_o, del_o, wbar_output;
       elem::Zeros(del_o, k, ni);
       LocalMatrixType Yp(Yv.Height(), k);
       LocalMatrixType Yp_labels(Yv.Height(), 1);

       /*LocalMatrixType wbar_tmp;
       //if (NumThreads > 1)

       elem::Zeros(wbar_tmp, k, ni);*/

       if (CacheTransforms)
                   InitializeTransformCache(ni);

       SKYLARK_TIMER_INITIALIZE(ITERATIONS_PROFILE);
       SKYLARK_TIMER_INITIALIZE(COMMUNICATION_PROFILE);
       SKYLARK_TIMER_INITIALIZE(TRANSFORM_PROFILE);
       SKYLARK_TIMER_INITIALIZE(ZTRANSFORM_PROFILE);
       SKYLARK_TIMER_INITIALIZE(ZMULT_PROFILE);
       SKYLARK_TIMER_INITIALIZE(PROXLOSS_PROFILE);
       SKYLARK_TIMER_INITIALIZE(BARRIER_PROFILE);
       SKYLARK_TIMER_INITIALIZE(PREDICTION_PROFILE);

       while(iter<MAXITER) {

           SKYLARK_TIMER_RESTART(ITERATIONS_PROFILE);

           iter++;

           SKYLARK_TIMER_RESTART(COMMUNICATION_PROFILE);
           broadcast(comm, Wbar.Buffer(), Dk, 0);

           SKYLARK_TIMER_ACCUMULATE(COMMUNICATION_PROFILE)

           // mu_ij = mu_ij - Wbar
           elem::Axpy(-1.0, Wbar, mu_ij);

           // Obar = Obar - nu
           elem::Axpy(-1.0, nu, Obar);

           SKYLARK_TIMER_RESTART(PROXLOSS_PROFILE);
           loss->proxoperator(Obar, 1.0/RHO, Y, O);
           SKYLARK_TIMER_ACCUMULATE(PROXLOSS_PROFILE);

           if(rank==0) {
               regularizer->proxoperator(Wbar, lambda/RHO, mu, W);
           }

           elem::Zeros(sum_o, k, ni);
           elem::Zeros(wbar_output, k, ni);

           int j;
           const feature_transform_t* featureMap;

           SKYLARK_TIMER_RESTART(TRANSFORM_PROFILE);

   #       ifdef SKYLARK_HAVE_OPENMP
   #       pragma omp parallel for if(NumThreads > 1) private(j, start, finish, sj, featureMap) num_threads(NumThreads)
   #       endif
           for(j = 0; j < NumFeaturePartitions; j++) {
               start = starts[j];
               finish = finishes[j];
               sj = finish - start  + 1;

               elem::Matrix<double> z(sj, ni);

               if (CacheTransforms && (iter > 1))
               {
                    elem::View(z,  *TransformCache[j], 0, 0, sj, ni);
               }
               else {
                   if (featureMaps.size() > 0) {
                       featureMap = featureMaps[j];

                       SKYLARK_TIMER_RESTART(ZTRANSFORM_PROFILE);
                       featureMap->apply(X, z, skylark::sketch::columnwise_tag());
                       SKYLARK_TIMER_ACCUMULATE(ZTRANSFORM_PROFILE)

                       if (ScaleFeatureMaps)
                           elem::Scal(sqrt(double(sj) / d), z);
                       } else {
                          // for linear case just use Z = X no slicing business.
                          // skylark::base::ColumnView<double>(z, x, );
                          // ;// VIEWS on SPARSE MATRICES: elem::View(z, x, start, 0, sj, ni);
                       }
               }

               elem::Matrix<double> tmp(sj, k);
               elem::Matrix<double> rhs(sj, k);
               elem::Matrix<double> o(k, ni);

               if(iter==1) {

                   elem::Matrix<double> Ones;
                   elem::Ones(Ones, sj, 1);
                   elem::Gemm(elem::NORMAL, elem::TRANSPOSE, 1.0, z, z, 0.0, *Cache[j]);
                   Cache[j]->UpdateDiagonal(Ones);
                   elem::Inverse(*Cache[j]);

                   if (CacheTransforms) {
                       *TransformCache[j] = z;
                       //DEBUG
                        std::cout << "CACHING TRANSFORMS..." << std::endl;
                        elem::Write(*TransformCache[0], "FeatureMatrix.asc", elem::ASCII, "");
                   }
               }

               elem::View(tmp, Wbar, start, 0, sj, k); //tmp = Wbar[J,:]

               LocalMatrixType wbar_tmp;
               elem::Zeros(wbar_tmp, k, ni);

               if (NumThreads > 1) {
                   elem::Gemm(elem::TRANSPOSE, elem::NORMAL, 1.0, tmp, z, 0.0, wbar_tmp);

   #               ifdef SKYLARK_HAVE_OPENMP
   #               pragma omp critical
   #               endif
                   elem::Axpy(1.0, wbar_tmp, wbar_output);
               } else
                   elem::Gemm(elem::TRANSPOSE, elem::NORMAL, 1.0, tmp, z, 1.0, wbar_output);

               rhs = tmp; //rhs = Wbar[J,:]
               elem::View(tmp, mu_ij, start, 0, sj, k); //tmp = mu_ij[J,:]
               elem::Axpy(-1.0, tmp, rhs); // rhs = rhs - mu_ij[J,:] = Wbar[J,:] - mu_ij[J,:]
               elem::View(tmp, ZtObar_ij, start, 0, sj, k);
               elem::Axpy(+1.0, tmp, rhs); // rhs = rhs + ZtObar_ij[J,:]

               SKYLARK_TIMER_RESTART(ZMULT_PROFILE);
               elem::Matrix<double> dsum = del_o;
               elem::Axpy(NumFeaturePartitions + 1.0, nu, dsum);
               elem::Gemm(elem::NORMAL, elem::TRANSPOSE, 1.0/(NumFeaturePartitions + 1.0), z, dsum, 1.0, rhs); // rhs = rhs + z'*(1/(n+1) * del_o + nu)
               SKYLARK_TIMER_ACCUMULATE(ZMULT_PROFILE);

               elem::View(tmp, Wi, start, 0, sj, k);
               elem::Gemm(elem::NORMAL, elem::NORMAL, 1.0, *Cache[j], rhs, 0.0, tmp); // ]tmp = Wi[J,:] = Cache[j]*rhs

               SKYLARK_TIMER_RESTART(ZMULT_PROFILE);
               elem::Gemm(elem::TRANSPOSE, elem::NORMAL, 1.0, tmp, z, 0.0, o); // o = (z*tmp)' = (z*Wi[J,:])'
               SKYLARK_TIMER_ACCUMULATE(ZMULT_PROFILE);

               // mu_ij[JJ,:] = mu_ij[JJ,:] + Wi[JJ,:];
               elem::View(tmp, mu_ij, start, 0, sj, k); //tmp = mu_ij[J,:]
               elem::View(rhs, Wi, start, 0, sj, k);
               elem::Axpy(+1.0, rhs, tmp);

               //ZtObar_ij[JJ,:] = numpy.dot(Z.T, o);
               elem::View(tmp, ZtObar_ij, start, 0, sj, k);
               elem::Gemm(elem::NORMAL, elem::TRANSPOSE, 1.0, z, o, 0.0, tmp);

               //  sum_o += o
               if (NumThreads > 1) {
   #               ifdef SKYLARK_HAVE_OPENMP
   #               pragma omp critical
   #               endif
                   elem::Axpy(1.0, o, sum_o);
               } else
                   elem::Axpy(1.0, o, sum_o);

               z.Empty();
           }

           SKYLARK_TIMER_ACCUMULATE(TRANSFORM_PROFILE);

           localloss = 0.0 ;
           //  elem::Zeros(o, ni, k);
           elem::Matrix<double> o(k, ni);
           elem::MakeZeros(o);
           elem::Scal(-1.0, sum_o);
           elem::Axpy(+1.0, O, sum_o); // sum_o = O.Matrix - sum_o
           del_o = sum_o;

           SKYLARK_TIMER_RESTART(PREDICTION_PROFILE);
           if (skylark::base::Width(Xv) > 0) {
               elem::MakeZeros(Yp);
               elem::MakeZeros(Yp_labels);
               model->predict(Xv, Yp_labels, Yp, NumThreads);
               accuracy = model->evaluate(Yv, Yp, comm);
           }
           SKYLARK_TIMER_ACCUMULATE(PREDICTION_PROFILE);

           localloss += loss->evaluate(wbar_output, Y);

           SKYLARK_TIMER_RESTART(COMMUNICATION_PROFILE);
           reduce(comm, localloss, totalloss, std::plus<double>(), 0);
           SKYLARK_TIMER_ACCUMULATE(COMMUNICATION_PROFILE);

           if(rank == 0) {
               obj = totalloss + lambda*regularizer->evaluate(Wbar);
               if (skylark::base::Width(Xv) <=0) {
                   std::cout << "iteration " << iter << " objective " << obj << " time " << timer.elapsed() << " seconds" << std::endl;
               }
               else {
                   std::cout << "iteration " << iter << " objective " << obj << " accuracy " << accuracy << " time " << timer.elapsed() << " seconds" << std::endl;
               }
           }

           elem::Copy(O, Obar);
           elem::Scal(1.0/(NumFeaturePartitions+1.0), sum_o);
           elem::Axpy(-1.0, sum_o, Obar);

           elem::Axpy(+1.0, O, nu);
           elem::Axpy(-1.0, Obar, nu);

           //Wbar = comm.reduce(Wi)
           SKYLARK_TIMER_RESTART(COMMUNICATION_PROFILE);
           boost::mpi::reduce (comm,
                                   Wi.LockedBuffer(),
                                   Wi.MemorySize(),
                                   Wbar.Buffer(),
                                   std::plus<double>(),
                                   0);
           SKYLARK_TIMER_ACCUMULATE(COMMUNICATION_PROFILE);

           if(rank==0) {
               //Wbar = (Wisum + W)/(P+1)
               elem::Axpy(1.0, W, Wbar);
               elem::Scal(1.0/(P+1), Wbar);

               // mu = mu + W - Wbar;
               elem::Axpy(+1.0, W, mu);
               elem::Axpy(-1.0, Wbar, mu);
           }

           SKYLARK_TIMER_RESTART(BARRIER_PROFILE);
           comm.barrier();
           SKYLARK_TIMER_ACCUMULATE(BARRIER_PROFILE);

           SKYLARK_TIMER_ACCUMULATE(ITERATIONS_PROFILE);
       }

       SKYLARK_TIMER_PRINT(ITERATIONS_PROFILE, comm);
       SKYLARK_TIMER_PRINT(COMMUNICATION_PROFILE, comm);
       SKYLARK_TIMER_PRINT(TRANSFORM_PROFILE, comm);
       SKYLARK_TIMER_PRINT(ZTRANSFORM_PROFILE, comm);
       SKYLARK_TIMER_PRINT(ZMULT_PROFILE, comm);
       SKYLARK_TIMER_PRINT(PROXLOSS_PROFILE, comm);
       SKYLARK_TIMER_PRINT(BARRIER_PROFILE, comm);
       SKYLARK_TIMER_PRINT(PREDICTION_PROFILE, comm);

       return model;
}

#endif /* SKYLARK_BLOCKADDM_HPP */


#pragma once
//! c/c++ headers
//! dependency headers
#include <armadillo>
#include "correspondences/common/types.hpp"  // for correspondences_t definition
//! project headers

namespace transforms {
/**
 * @brief Identify best transformation between two sets of points with known correspondences
 *
 * @param [in] src_pts points to transform
 * @param [in] dst_pts target points
 * @param [in] corrs mapping of indices from src_pts to dst_pts
 * @param [in][out] H_optimal best-fit transformation to align points in homogeneous coordinates
 * @return
 *
 * @note src_pts and dst_pts must have the same number of columns
 */
bool best_fit_transform(arma::mat const & src_pts, arma::mat const & dst_pts,
    correspondences::correspondences_t const & corrs, arma::mat44 & H_optimal) noexcept;
}  // namespace transforms


#include "BVH.h"
#include <iostream>
#include <sstream>
#include <Eigen/Geometry>
#include "dart/dart.hpp"

using namespace dart::dynamics;

namespace MASS
{

Eigen::Matrix3d R_x(double x)
{
	double cosa = cos(x * M_PI / 180.0);
	double sina = sin(x * M_PI / 180.0);
	Eigen::Matrix3d R;
	R << 1, 0, 0,
		0, cosa, -sina,
		0, sina, cosa;
	return R;
}
Eigen::Matrix3d R_y(double y)
{
	double cosa = cos(y*M_PI/180.0);
	double sina = sin(y*M_PI/180.0);
	Eigen::Matrix3d R;
	R <<cosa ,0,sina,
		0    ,1,   0,
		-sina,0,cosa;
	return R;
}
Eigen::Matrix3d R_z(double z)
{
	double cosa = cos(z*M_PI/180.0);
	double sina = sin(z*M_PI/180.0);
	Eigen::Matrix3d R;
	R<<	cosa,-sina,0,
		sina,cosa ,0,
		0   ,0    ,1;
	return R;
}

BVHNode::BVHNode(const std::string &name, BVHNode *parent)
	:mParent(parent),mName(name),mChannelOffset(0),mNumChannels(0)
{
	mChannel.clear();
	mChildren.clear();
}

void BVHNode::SetChannel(int c_offset, std::vector<std::string>& c_name)
{
	mChannelOffset = c_offset;
	mNumChannels = c_name.size();
	for (const auto& cn : c_name)
		mChannel.push_back(CHANNEL_NAME[cn]);
}

void BVHNode::Set(const Eigen::VectorXd& m_t)
{
	mR.setIdentity();

	for (int i=0;i<mNumChannels;i++)
	{
		switch(mChannel[i])
		{
		case Xpos:
			break;
		case Ypos:
			break;
		case Zpos:
			break;
		case Xrot:
			mR = mR*R_x(m_t[mChannelOffset+i]);
			break;
		case Yrot:
			mR = mR*R_y(m_t[mChannelOffset+i]);
			break;
		case Zrot:
			mR = mR*R_z(m_t[mChannelOffset+i]);
			break;
		default:
			break;
		}
	}

}

void BVHNode::Set(const Eigen::Matrix3d& R_t)
{
	mR = R_t;
}

Eigen::Matrix3d BVHNode::Get()
{
	return mR;
}

void BVHNode::AddChild(BVHNode* child)
{
	mChildren.push_back(child);
}

BVHNode* BVHNode::GetNode(const std::string& name)
{
	if (!mName.compare(name))
		return this;

	for (auto& c : mChildren)
	{
		auto bn = c->GetNode(name);
		if (bn != nullptr)
			return bn;
	}

	return nullptr;
}

BVH::BVH(const dart::dynamics::SkeletonPtr& skel,const std::map<std::string,std::string>& bvh_map)
	:mSkeleton(skel), mBVHMap(bvh_map), mCyclic(true)
{
	mBVHToSkelMap.clear();
	for (auto &i : bvh_map)
		mBVHToSkelMap[i.second] = i.first;
	mNodes.clear();
	mNodeNames.clear();
}

BVH::~BVH()
{
	mMap.clear();
	for (size_t i = 0; i < mNodes.size(); i++)
	{
		if (nullptr != mNodes[i])
		{
			delete mNodes[i];
		}
	}
	mNodes.clear();
}

Eigen::VectorXd BVH::GetMotion(double t)
{
	int k = ((int)std::floor(t/mTimeStep));
	if(mCyclic)
		k %= mNumTotalFrames;
	k = std::max(0,std::min(k,mNumTotalFrames-1));
	double dt = t/mTimeStep - std::floor(t/mTimeStep);
	Eigen::VectorXd m_t = mMotions[k];

	for(auto& bn: mMap)
		bn.second->Set(m_t);

	int dof = mSkeleton->getNumDofs();
	Eigen::VectorXd p = Eigen::VectorXd::Zero(dof);

	for(auto ss : mBVHMap)
	{
		BodyNode* bn = mSkeleton->getBodyNode(ss.first);
		Eigen::Matrix3d R = this->Get(ss.second);
		Joint* jn = bn->getParentJoint();
		int idx = jn->getIndexInSkeleton(0);

		if(jn->getType()=="FreeJoint")
		{
			Eigen::Isometry3d T;
			T.translation() = 0.01*m_t.segment<3>(0);
			T.linear() = R;
			p.segment<6>(idx) = FreeJoint::convertToPositions(T);
		}
		else if(jn->getType()=="BallJoint")
			p.segment<3>(idx) = BallJoint::convertToPositions(R);
		else if(jn->getType()=="RevoluteJoint")
		{
			Eigen::Vector3d u =dynamic_cast<RevoluteJoint*>(jn)->getAxis();
			Eigen::Vector3d aa = BallJoint::convertToPositions(R);
			double val;
			if((u-Eigen::Vector3d::UnitX()).norm()<1E-4)
				val = aa[0];
			else if((u-Eigen::Vector3d::UnitY()).norm()<1E-4)
				val = aa[1];
			else
				val = aa[2];

			if(val>M_PI)
				val -= 2*M_PI;
			else if(val<-M_PI)
				val += 2*M_PI;

			p[idx] = val;
		}
	}

	return p;
}

Eigen::Matrix3d BVH::Get(const std::string& bvh_node)
{
	return mMap[bvh_node]->Get();
}

void BVH::Parse(const std::string& file, bool cyclic)
{
	std::ifstream is(file);
	if (!is)
	{
		std::cout << "Can't Open File" << std::endl;
		return;
	}
	else {
		_Parse(is, cyclic);
	}
	is.close();
}

void BVH::ParseStr(const std::string &str, bool cyclic)
{
	std::stringstream is;
    is.str(str);
	_Parse(is, cyclic);
}

void BVH::_Parse(std::istream &is, bool cyclic)
{
    mCyclic = cyclic;

    char buffer[256];

    mMap.clear();
	for (size_t i = 0; i < mNodes.size(); i++)
	{
		if (nullptr != mNodes[i])
		{
			delete mNodes[i];
		}
	}
	mNodes.clear();
	mNodeNames.clear();
	mMotions.clear();
	while (is >> buffer)
	{
		if (!strcmp(buffer, "HIERARCHY"))
		{
			is >> buffer; //Root
			is >> buffer; //Name
			int c_offset = 0;
			mRoot = ReadHierarchy(nullptr, buffer, c_offset, is);
			mNumTotalChannels = c_offset;
		}
		else if (!strcmp(buffer, "MOTION"))
		{
			is >> buffer; //Frames:
			is >> buffer; //num_frames
			mNumTotalFrames = atoi(buffer);
			is >> buffer; //Frame
			is >> buffer; //Time:
			is >> buffer; //time step
			mTimeStep = atof(buffer);
			mMotions.resize(mNumTotalFrames);
			for (auto& m_t : mMotions)
				m_t = Eigen::VectorXd::Zero(mNumTotalChannels);
			double val;
			for (int i = 0; i < mNumTotalFrames; i++)
			{
				for (int j = 0; j < mNumTotalChannels; j++)
				{
					is >> val;
					mMotions[i][j] = val;
				}
			}
		}
	}

	BodyNode* root = mSkeleton->getRootBodyNode();
	std::string root_bvh_name = mBVHMap[root->getName()];
	Eigen::VectorXd m = mMotions[0];

	mMap[root_bvh_name]->Set(m);
	T0.linear() = this->Get(root_bvh_name);
	T0.translation() = 0.01 * m.head<3>();

	m = mMotions[mNumTotalFrames - 1];

	mMap[root_bvh_name]->Set(m);
	T1.linear() = this->Get(root_bvh_name);
	T1.translation() = 0.01 * m.head<3>();
}

BVHNode* BVH::ReadHierarchy(BVHNode* parent,const std::string& name,int& channel_offset,std::istream& is)
{
    char buffer[256];
    double offset[3];
	std::vector<std::string> c_name;

	mNodes.push_back(new BVHNode(name, parent));
	auto new_node = mNodes.back();
    mMap.insert(std::make_pair(name, new_node));
	if(name.find("EndEffector") != std::string::npos)
		mNodeNames.push_back(name);

	is>>buffer; //{

	while (is >> buffer)
	{
		if (!strcmp(buffer, "}"))
			break;
		if (!strcmp(buffer, "OFFSET"))
		{
			//Ignore
			double x, y, z;

			is >> x;
			is >> y;
			is >> z;
		}
		else if (!strcmp(buffer, "CHANNELS"))
		{
			is >> buffer;
			int n;
			n = atoi(buffer);

			for (int i = 0; i < n; i++)
			{
				is >> buffer;
				c_name.push_back(std::string(buffer));
			}

			new_node->SetChannel(channel_offset, c_name);
			channel_offset += n;
		}
		else if (!strcmp(buffer, "JOINT"))
		{
			is >> buffer;
			auto child = ReadHierarchy(new_node, std::string(buffer), channel_offset, is);
			new_node->AddChild(child);
		}
		else if (!strcmp(buffer, "End"))
		{
			is >> buffer;
			auto child = ReadHierarchy(new_node, std::string(buffer) + std::string("EndEffector"), channel_offset, is);
			new_node->AddChild(child);
		}
    }

    return new_node;
}

std::map<std::string, MASS::BVHNode::CHANNEL> BVHNode::CHANNEL_NAME =
{
	{"Xposition", Xpos},
	{"XPOSITION", Xpos},
	{"Yposition", Ypos},
	{"YPOSITION", Ypos},
	{"Zposition", Zpos},
	{"ZPOSITION", Zpos},
	{"Xrotation", Xrot},
	{"XROTATION", Xrot},
	{"Yrotation", Yrot},
	{"YROTATION", Yrot},
	{"Zrotation", Zrot},
	{"ZROTATION", Zrot}
};
};


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

#ifndef TEST_KALMAN_FILTER_HPP_
#define TEST_KALMAN_FILTER_HPP_

#include <common/types.hpp>
#include <Eigen/Cholesky>
#include <chrono>
#include <limits>
#include "kalman_filter/srcf_core.hpp"
#include "kalman_filter/esrcf.hpp"
#include "motion_model/constant_velocity.hpp"
#include "motion_model/parameter_estimator.hpp"

using autoware::prediction::kalman_filter::SrcfCore;
using autoware::prediction::kalman_filter::Esrcf;
using autoware::motion::motion_model::ConstantVelocity;
using autoware::motion::motion_model::ParameterEstimator;
using Eigen::Matrix;
using autoware::common::types::float32_t;
const float32_t TOL = 1.0E-6F;

template<typename T, uint64_t H, uint64_t W>
void print(const Matrix<T, H, W> & A)
{
  std::cerr << "\nPrint Matrix:\n\n";
  for (uint64_t i = 0; i < H; ++i) {
    for (uint64_t j = 0; j < W; ++j) {
      std::cerr << A(i, j) << ", \t";
    }
    std::cerr << "\n";
  }
}

// example 5.4, in Kalman Filtering Theory and Practice using Matlab, pg 193-194
TEST(srcf_core, univariate)
{
  Matrix<float32_t, 1, 1> F, H, Q, R, x, z, P, B, G;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  const float32_t log2pi = 1.83787706641F;
  EXPECT_FLOAT_EQ(log2pi, logf(3.14159265359F * 2.0F));
  F << 1;
  H << 1;
  Q << 1;  // sqrt, but 1 so whatever
  R << 2;
  x << 1;
  P << sqrtf(10.0F);
  G << 1;  // no mapping from procss noise subspace to state space
  SrcfCore<1, 1> core;
  // temporal update 0->1
  P(0) = F(0) * P(0);
  B(0) = G(0) * Q(0);
  core.right_lower_triangularize_matrices(P, B);
  EXPECT_LT(fabsf(B(0) - 0.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(11.0F)), TOL);
  // observation update 1
  z(0) = 2;
  float32_t S = (H(0) * (P(0) * P(0)) * H(0)) + R(0);
  float32_t err = z(0) - (H(0) * x(0));
  float32_t pdf = -0.5F * (log2pi + logf(S) + (err * err) / S);
  EXPECT_LT(fabsf(core.scalar_update(z(0), R(0), H, P, x) - pdf), TOL) << pdf;
  EXPECT_LT(fabsf(x(0) - 24.0F / 13.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(22.0 / 13.0)), TOL);
  // temporal update 1->2
  P(0) = F(0) * P(0);
  B(0) = G(0) * Q(0);
  core.right_lower_triangularize_matrices(P, B);
  EXPECT_LT(fabsf(B(0) - 0.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(35.0F / 13.0F)), TOL);
  // observation update 2
  z(0) = 3;
  S = (H(0) * (P(0) * P(0)) * H(0)) + R(0);
  err = z(0) - (H(0) * x(0));
  pdf = -0.5F * (log2pi + logf(S) + (err * err) / S);
  EXPECT_LT(fabsf(core.scalar_update(z(0), R(0), H, P, x) - pdf), TOL) << pdf;
  EXPECT_LT(fabsf(x(0) - 153.0F / 61.0F), TOL);
  EXPECT_LT(fabsf(P(0) - sqrtf(70.0F / 61.0F)), TOL);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
  // test bad case
  EXPECT_THROW(core.scalar_update(0.0F, 0.0F, H, P, x), std::domain_error);
}

// example 5.5 in Kalman Filtering Theory and Pracice using Matlab, pg 195-196
TEST(srcf_core, multivariate)
{
  Matrix<float32_t, 2, 1> x({1, 2}), z({3, 4});
  Matrix<float32_t, 2, 2> H, C, R;
  H << 0, 2, 3, 0;
  C << 4, 1, 1, 9;
  R << 1, 0, 0, 4;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  // higher TOL to match matlab precision
  const float32_t TOL2 = 1.0E-4F;
  Eigen::LLT<Eigen::Ref<decltype(C)>> llt(C);

  C(0U, 1U) = 0.0F;
  // cholesky from numpy
  EXPECT_LT(fabsf(C(0, 0) - 2), TOL);
  EXPECT_LT(fabsf(C(1, 0) - 0.5F), TOL);
  EXPECT_LT(fabsf(C(1, 1) - 2.95803989F), TOL);
  // do vector update
  SrcfCore<2, 2> core;
  // observation update 1a
  float32_t likelihood = core.scalar_update(z(0), R(0, 0), H.row(0), C, x);
  //// check intermediate values
  // These values are from the worked example in the book
  EXPECT_LT(fabsf(x(0) - (35.0 / 37.0)), TOL2);
  EXPECT_LT(fabsf(x(1) - (56.0 / 37.0)), TOL2);
  // check C, computed manually with scipy
  EXPECT_LT(fabsf(C(0, 0) - (1.97278785)), TOL2);
  EXPECT_LT(fabsf(C(1, 0) - (0.01369992)), TOL2);
  EXPECT_LT(fabsf(C(1, 1) - (0.49300665)), TOL2);
  // observation update 1b
  likelihood += core.scalar_update(z(1), R(1, 1), H.row(1), C, x);
  //// check final values
  // These values are from the worked example in the book
  EXPECT_LT(fabsf(x(0) - (467.0 / 361.0)), TOL2);
  EXPECT_LT(fabsf(x(1) - (2189.0 / 1444.0)), TOL2);
  // check C
  EXPECT_LT(fabsf(C(0, 0) - (0.63157895)), TOL2);
  EXPECT_LT(fabsf(C(1, 0) - (0.00438596)), TOL2);
  EXPECT_LT(fabsf(C(1, 1) - (0.49300665)), TOL2);
  // check likelihood of exact matrix:
  // S = H * P * H' + R = [37, 6; 6, 40]
  // err = z - (H * x) = [-1; 1]
  // P(err | 0, S) = -5.506280400650966 (from scipy)
  EXPECT_LT(fabsf(likelihood - (-5.506280400650966)), 0.01F) << likelihood;
  // Close enough, < 1% relative error
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// example from Thornton's thesis (see below), pg 28
// This is a case where the conventional kalman filter will fail but the square root will not
// Because the example uses a different factorization paradigm, the answers in thes Thesis
// don't work (they use P = U * U^T, modern standard is P = U^T * U)
/*
s = some big number
e = 1/s = some small number
P = [s^2, 0; 0, s^2]
H = [1, e; 1, 1]
R = [1, 1]
P1 = [
⎡    2        3     ⎤
⎢ 2⋅s       -s      ⎥
⎢──────    ──────   ⎥
⎢ 2         2       ⎥
⎢s  + 2    s  + 2   ⎥
⎢                   ⎥ ~ [2    -s ]
⎢   3     2 ⎛ 2    ⎞⎥   [-s   s^2]
⎢ -s     s ⋅⎝s  + 1⎠⎥
⎢──────  ───────────⎥
⎢ 2          2      ⎥
⎣s  + 2     s  + 2  ⎦
S1 = L1 =
⎡           ________              ⎤
⎢          ╱    2                 ⎥
⎢         ╱    s                  ⎥
⎢  √2⋅   ╱   ──────         0     ⎥
⎢       ╱     2                   ⎥
⎢     ╲╱     s  + 2               ⎥
⎢                                 ⎥
⎢            ________             ⎥ ~ [sqrtf(2)         0             ]
⎢           ╱    2                ⎥   [-sqrf(2)/2 * s   sqrtf(2)/2 * s]
⎢          ╱    s                 ⎥
⎢-√2⋅s⋅   ╱   ──────          ____⎥
⎢        ╱     2             ╱  2 ⎥
⎢      ╲╱     s  + 2    √2⋅╲╱  s  ⎥
⎢─────────────────────  ──────────⎥
⎣          2                2     ⎦

Above were computed exactly using sympy

Similarly, the factors end up being
⎡  _________                       ⎤
⎢╲╱ 2⋅e + 1             0          ⎥
⎢                                  ⎥
⎢                  ________________⎥
⎢                 ╱    2           ⎥
⎢-(3⋅e + 1)      ╱  - e  + 2⋅e + 1 ⎥
⎢───────────    ╱   ────────────── ⎥
⎢  _________  ╲╱       2⋅e + 1     ⎥
⎣╲╱ 2⋅e + 1                        ⎦

for the second update
*/
TEST(srcf_core, degenerate)
{
  using Vector2f = Matrix<float32_t, 2, 1>;
  using Matrix2f = Matrix<float32_t, 2, 2>;
  const float32_t eps = 1.0E-6F;
  EXPECT_LT(fabsf((1.0F + (eps * eps)) - 1), TOL);
  const float32_t sigma = 1.0F / eps;
  Matrix2f H{(Matrix2f{} << 1, eps, 1, 1).finished()};
  Matrix2f C{(Matrix2f{} <<
      sigma * sigma, 0,
      0, sigma * sigma).finished()};
  Vector2f R{1, 1};
  Vector2f x{Vector2f::Zero()};
  // cholesky on C
  Eigen::LLT<Eigen::Ref<decltype(C)>> llt(C);

  C(0, 1) = 0.0F;
  SrcfCore<2, 2> core;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  // update
  float32_t ll = core.scalar_update(0, R(0), H.row(0), C, x);
  EXPECT_LT(fabsf(C(0, 0) - sqrtf(2)), TOL) << "C matrix: " << C;
  EXPECT_LT(fabsf(C(1, 0) - (-sqrtf(2) * sigma / 2)), TOL);
  EXPECT_LT(fabsf(C(1, 1) - sqrtf(2) * sigma / 2), TOL);
  ll += core.scalar_update(0, R(1), H.row(1), C, x);
  EXPECT_LT(fabsf(C(0, 0) - (1)), TOL);
  EXPECT_LT(fabsf(C(1, 0) - (-1 - 3 * eps)), 0.1F);  // This one is hard to compute exactly
  EXPECT_LT(fabsf(C(1, 1) - (1)), TOL);
  // S = [sig^2, sig^2; sig^2, 2*sig^2]
  // z = err = x = 0
  EXPECT_LT(fabsf(ll - (-29.468897182338893)), 1.0E-5F) << ll;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// Example 3.1 in page 66 in NASA JPL paper:
// https://ntrs.nasa.gov/archive/nasa/casi.ntrs.nasa.gov/19770005172.pdf
// Modified from reference, since they use U*U^T = P, where convention is U^T * U = P
// test covariance temporal update
TEST(srcf_core, propagation)
{
  Matrix<float32_t, 2, 1> GQ({0, 1});  // B = [0; 1], Q = 1
  const float32_t eps = 1.0E-5F;
  const float32_t sigma = 1.0F / eps;
  // sigma ^2 + 1 ~= sigma ^2
  EXPECT_LT(fabsf((sigma * sigma + 1) - (sigma * sigma)), TOL);
  Matrix<float32_t, 2, 2> C;
  C << sigma, 0, sigma, 1;
  SrcfCore<2, 1> core;
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  core.right_lower_triangularize_matrices(C, GQ);
  EXPECT_LT(fabsf(C(1, 1) - (sqrtf(2))), eps) << C(0, 0);
  EXPECT_LT(fabsf(C(1, 0) - (sigma)), eps) << C(1, 0);
  EXPECT_LT(fabsf(C(0, 0) - (sigma)), eps) << C(1, 1);
  // B should be zero'd
  EXPECT_LT(fabsf(GQ(0)), eps) << GQ(0);
  EXPECT_LT(fabsf(GQ(1)), eps) << GQ(1);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// test other branches of givens rotations
TEST(srcf_core, triangularization)
{
  SrcfCore<3, 1> core;
  Matrix<float32_t, 3, 3> A;
  A <<
    1, 1, 1,
    1, -1, 1,
    1, 1, 0;
  Matrix<float32_t, 3, 3> C(A), D(A), E(A);
  Matrix<float32_t, 3, 1> B;  // zero
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  core.right_lower_triangularize_matrices(A, B);
  // B should remain 0
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(A(0, 1)), TOL);
  EXPECT_LT(fabsf(A(0, 2)), TOL);
  EXPECT_LT(fabsf(A(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(A(0, 0), NAN);
  EXPECT_NE(A(1, 0), NAN);
  EXPECT_NE(A(2, 0), NAN);
  EXPECT_NE(A(1, 1), NAN);
  EXPECT_NE(A(2, 1), NAN);
  EXPECT_NE(A(2, 2), NAN);
  //// yet another case
  E(0, 0) = 0;
  E(2, 0) = -1;
  core.right_lower_triangularize_matrices(E, B);
  // B should remain 0
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(E(0, 1)), TOL);
  EXPECT_LT(fabsf(E(0, 2)), TOL);
  EXPECT_LT(fabsf(E(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(E(0, 0), NAN);
  EXPECT_NE(E(1, 0), NAN);
  EXPECT_NE(E(2, 0), NAN);
  EXPECT_NE(E(1, 1), NAN);
  EXPECT_NE(E(1, 2), NAN);
  EXPECT_NE(E(2, 2), NAN);
  // another arbitrary case
  B(2) = -1;
  C(2, 1) = -1;
  core.right_lower_triangularize_matrices(C, B);
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(C(0, 1)), TOL);
  EXPECT_LT(fabsf(C(0, 2)), TOL);
  EXPECT_LT(fabsf(C(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(C(0, 0), NAN);
  EXPECT_NE(C(1, 0), NAN);
  EXPECT_NE(C(2, 0), NAN);
  EXPECT_NE(C(1, 1), NAN);
  EXPECT_NE(C(1, 2), NAN);
  EXPECT_NE(C(2, 2), NAN);
  // one more case
  B(0) = -1;
  B(1) = -2;
  B(2) = 1;
  D(0, 0) = 0;
  D(1, 1) = 0;
  core.right_lower_triangularize_matrices(D, B);
  EXPECT_LT(fabsf(B(0)), TOL);
  EXPECT_LT(fabsf(B(1)), TOL);
  EXPECT_LT(fabsf(B(2)), TOL);
  // lower triangle of A should be 0
  EXPECT_LT(fabsf(D(0, 1)), TOL);
  EXPECT_LT(fabsf(D(0, 2)), TOL);
  EXPECT_LT(fabsf(D(1, 2)), TOL);
  // Not NAN
  EXPECT_NE(D(0, 0), NAN);
  EXPECT_NE(D(1, 0), NAN);
  EXPECT_NE(D(2, 0), NAN);
  EXPECT_NE(D(1, 1), NAN);
  EXPECT_NE(D(1, 2), NAN);
  EXPECT_NE(D(2, 2), NAN);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

// given a correct initial guess and perfect observations, state
// should not change and covariance should shrink to steady state
TEST(esrcf, convergence)
{
  ConstantVelocity model;
  Matrix<float32_t, 4, 1> R({1, 1, 1, 1});
  // identity
  Matrix<float32_t, 4, 4> GQ;
  GQ <<
    0.1F, 0.0F, 0.0F, 0.0F,
    0.0F, 0.1F, 0.0F, 0.0F,
    0.0F, 0.0F, 0.1F, 0.0F,
    0.0F, 0.0F, 0.0F, 0.1F;

  Matrix<float32_t, 4, 1> x({0, 0, 1, -1});  // all 0's
  // cholesky of: (so there is some covariance wrt hidden state
  // 1   0   0.5 0
  // 0   1   0   0.5
  // 0.5 0   1   0
  // 0   0.5 0   1
  Matrix<float32_t, 4, 4> P;
  P <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1;
  // prefit and postfit covariance matrices
  Matrix<float32_t, 4, 4> P_m(P), P_p(P), P_last;
  const Matrix<float32_t, 4, 4> H((Matrix<float32_t, 4, 4>() <<
    1, 0, 0, 0,
    0, 1, 0, 0,
    0, 0, 1, 0,
    0, 0, 0, 1 ).finished());

  Esrcf<4, 4> kf(model, GQ);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  kf.reset(x, P);
  float32_t last_ll = -std::numeric_limits<float32_t>::max();
  // microseconds_100 = 0.1s
  std::chrono::nanoseconds microseconds_100(100000000LL);
  for (uint32_t iteration = 0; iteration < 30; ++iteration) {
    x(0) += 0.1;
    x(1) -= 0.1;
    kf.temporal_update(microseconds_100);
    // covariance should grow wrt postfit
    EXPECT_GT(kf.get_covariance()(0, 0), P_p(0, 0));
    EXPECT_GT(kf.get_covariance()(1, 1), P_p(1, 1));
    EXPECT_GT(kf.get_covariance()(2, 2), P_p(2, 2));
    EXPECT_GT(kf.get_covariance()(3, 3), P_p(3, 3));
    // covariance should be smaller than last prefit
    if (iteration > 0) {
      EXPECT_LE(kf.get_covariance()(0, 0), P_m(0, 0));
      EXPECT_LE(kf.get_covariance()(1, 1), P_m(1, 1));
      EXPECT_LE(kf.get_covariance()(2, 2), P_m(2, 2));
      EXPECT_LE(kf.get_covariance()(3, 3), P_m(3, 3));
    }
    P_m = kf.get_covariance();
    // state should still be 0
    EXPECT_LT(fabsf(model[0] - x(0)), TOL);
    EXPECT_LT(fabsf(model[1] - x(1)), TOL);
    EXPECT_LT(fabsf(model[2] - x(2)), TOL);
    EXPECT_LT(fabsf(model[3] - x(3)), TOL);
    //// exact observation
    const float32_t ll = kf.observation_update(x, H, R);
    // likelihood should improve
    EXPECT_GT(ll, last_ll);
    last_ll = ll;
    // covariance should shrink
    EXPECT_LE(kf.get_covariance()(0, 0), P_m(0, 0));
    EXPECT_LE(kf.get_covariance()(1, 1), P_m(1, 1));
    EXPECT_LE(kf.get_covariance()(2, 2), P_m(2, 2));
    EXPECT_LE(kf.get_covariance()(3, 3), P_m(3, 3));
    // covariance should also be smaller wrt last postfit
    EXPECT_LE(kf.get_covariance()(0, 0), P_p(0, 0));
    EXPECT_LE(kf.get_covariance()(1, 1), P_p(1, 1));
    EXPECT_LE(kf.get_covariance()(2, 2), P_p(2, 2));
    EXPECT_LE(kf.get_covariance()(3, 3), P_p(3, 3));
    P_last = P_p;
    P_p = kf.get_covariance();
    // state should still be 0
    EXPECT_LT(fabsf(model[0] - x(0)), TOL);
    EXPECT_LT(fabsf(model[1] - x(1)), TOL);
    EXPECT_LT(fabsf(model[2] - x(2)), TOL);
    EXPECT_LT(fabsf(model[3] - x(3)), TOL);
  }
  P_last -= P_p;
  float32_t norm = 0.0F;
  for (uint32_t i = 0; i < P_last.rows(); ++i) {
    for (uint32_t j = 0; j < P_last.cols(); ++j) {
      norm += P_last(i, j) * P_last(i, j);
    }
  }
  EXPECT_LE(sqrtf(norm), 0.0005);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

/// @test Hidden states should converge to a good value
TEST(esrcf, hidden_state)
{
  constexpr float32_t kConvergenceEpsilon{0.001F};
  constexpr auto kIndexX = ConstantVelocity::States::POSE_X;
  constexpr auto kIndexY = ConstantVelocity::States::POSE_Y;
  constexpr auto kIndexSpeedX = ConstantVelocity::States::VELOCITY_X;
  constexpr auto kIndexSpeedY = ConstantVelocity::States::VELOCITY_Y;
  using FloatSeconds = std::chrono::duration<float32_t>;
  ConstantVelocity model;
  Matrix<float32_t, 2, 1> R({0.01F, 0.01F});
  const std::chrono::milliseconds dt{100LL};
  const FloatSeconds time_step_seconds{dt};
  // We use the Wiener model noise in this test. In this case, we only set noise on acceleration and
  // the matrix G is responsible to distribute it over to speed and position components of the
  // state.
  Matrix<float32_t, 4, 2> G;
  G <<
    0.5F * time_step_seconds.count() * time_step_seconds.count(), 0.0F,
    0.0F, 0.5F * time_step_seconds.count() * time_step_seconds.count(),
    time_step_seconds.count(), 0.0F,
    0.0F, time_step_seconds.count();
  Matrix<float32_t, 4, 4> F;
  model.compute_jacobian(F, dt);
  const auto acceleration_variance{2.0F};
  const auto GQ = F * G * acceleration_variance;
  Matrix<float32_t, 4, 1> x{Matrix<float32_t, 4, 1>::Zero()};  // all 0's
  Matrix<float32_t, 4, 4> P;
  P <<
    1.0F, 0.0F, 0.0F, 0.0F,
    0.0F, 1.0F, 0.0F, 0.0F,
    0.0F, 0.0F, 1.0F, 0.0F,
    0.0F, 0.0F, 0.0F, 1.0F;
  Esrcf<4, 2> kf(model, GQ, x, P);
  // prefit and postfit covariance matrices
  auto covariance_postfit{P};
  const Matrix<float32_t, 4, 4> P0{P};
  const Matrix<float32_t, 2, 4> H{(Matrix<float32_t, 2, 4>{} <<
      1, 0, 0, 0,
      0, 1, 0, 0).finished()};
  Matrix<float32_t, 2, 1> z{0.0F, 0.0F};
  const float32_t vx = 1.0F;
  const float32_t vy = 1.0F;
  EXPECT_NE(model[ConstantVelocity::States::VELOCITY_X], vx);
  EXPECT_NE(model[ConstantVelocity::States::VELOCITY_Y], vy);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  float32_t err_u = std::numeric_limits<float32_t>::max();
  float32_t err_v = std::numeric_limits<float32_t>::max();
  float32_t last_ll = -std::numeric_limits<float32_t>::max();

  auto converged = false;
  for (auto iteration = 0U; iteration < 100U; ++iteration) {
    z(ConstantVelocity::States::POSE_X) += time_step_seconds.count() * vx;
    z(ConstantVelocity::States::POSE_Y) += time_step_seconds.count() * vy;
    kf.temporal_update(dt);
    // Covariance should grow wrt postfit unless we have converged.
    const auto cov_after_prediction{kf.get_covariance() * kf.get_covariance().transpose()};
    EXPECT_FLOAT_EQ(
      cov_after_prediction(kIndexX, kIndexX),
      cov_after_prediction(kIndexY, kIndexY)) <<
      "Position variances should be the same in this test";
    EXPECT_FLOAT_EQ(
      cov_after_prediction(kIndexSpeedX, kIndexSpeedX),
      cov_after_prediction(kIndexSpeedY, kIndexSpeedY)) <<
      "Speed variances should be the same in this test";
    EXPECT_GT(cov_after_prediction(kIndexX, kIndexX), covariance_postfit(kIndexX, kIndexX));
    EXPECT_GT(cov_after_prediction(kIndexY, kIndexY), covariance_postfit(kIndexY, kIndexY));
    EXPECT_GT(
      cov_after_prediction(kIndexSpeedX, kIndexSpeedX),
      covariance_postfit(kIndexSpeedX, kIndexSpeedX));
    EXPECT_GT(
      cov_after_prediction(kIndexSpeedY, kIndexSpeedY),
      covariance_postfit(kIndexSpeedY, kIndexSpeedY));
    // Hidden state error should shrink
    float32_t err = fabsf(model[ConstantVelocity::States::VELOCITY_X] - vx);
    EXPECT_LT(err, err_u) << iteration;
    err_u = err;
    err = fabsf(model[ConstantVelocity::States::VELOCITY_Y] - vy);
    EXPECT_LT(err, err_v) << iteration;
    err_v = err;
    if ((err_u < kConvergenceEpsilon) && (err_v < kConvergenceEpsilon)) {
      converged = true;
      break;
    }
    const auto cov_factor_prefit{kf.get_covariance()};
    // exact observation
    const float32_t ll = kf.observation_update(z, H, R);
    // likelihood should improve or converge
    EXPECT_GE(ll, last_ll) << iteration;
    last_ll = ll;
    // covariance should shrink or converge
    EXPECT_LE(kf.get_covariance()(kIndexX, kIndexX), cov_factor_prefit(kIndexX, kIndexX));
    EXPECT_LE(kf.get_covariance()(kIndexY, kIndexY), cov_factor_prefit(kIndexY, kIndexY));
    EXPECT_LE(
      kf.get_covariance()(kIndexSpeedX, kIndexSpeedX),
      cov_factor_prefit(kIndexSpeedX, kIndexSpeedX));
    EXPECT_LE(
      kf.get_covariance()(kIndexSpeedY, kIndexSpeedY),
      cov_factor_prefit(kIndexSpeedY, kIndexSpeedY));
    // Postfit covariance should be smaller than p0 after some iterations.
    if (iteration > 10) {
      EXPECT_LT(kf.get_covariance()(kIndexX, kIndexX), P0(kIndexX, kIndexX));
      EXPECT_LT(kf.get_covariance()(kIndexY, kIndexY), P0(kIndexY, kIndexY));
      EXPECT_LT(kf.get_covariance()(kIndexSpeedX, kIndexSpeedX), P0(kIndexSpeedX, kIndexSpeedX));
      EXPECT_LT(kf.get_covariance()(kIndexSpeedY, kIndexSpeedY), P0(kIndexSpeedY, kIndexSpeedY));
    }
    covariance_postfit = kf.get_covariance() * kf.get_covariance().transpose();
    EXPECT_FLOAT_EQ(
      covariance_postfit(kIndexX, kIndexX),
      covariance_postfit(kIndexY, kIndexY)) <<
      "Position variances should be the same in this test";
    EXPECT_FLOAT_EQ(
      covariance_postfit(kIndexSpeedX, kIndexSpeedX),
      covariance_postfit(kIndexSpeedY, kIndexSpeedY)) <<
      "Speed variances should be the same in this test";
    // state should be very close to observation
    EXPECT_LT(fabsf(model[kIndexX] - z(kIndexX)), kf.get_covariance()(kIndexX, kIndexX));
    EXPECT_LT(fabsf(model[kIndexY] - z(kIndexY)), kf.get_covariance()(kIndexY, kIndexY));
  }
  EXPECT_TRUE(converged);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}

TEST(esrcf, imm_mix)
{
  static const uint32_t dim = 2U;
  Matrix<float32_t, dim, dim> P1, P2, P, C, GQ;
  Matrix<float32_t, dim, 1> x1, x2, x_mix;
  P1 << 2.0F, 2.0F, 2.0F, 4.0F;
  P2 << 2.0F, 1.0F, 1.0F, 2.0F;
  C = P1;
  Eigen::LLT<Eigen::Ref<decltype(C)>> llt(C);

  C(0U, 1U) = 0.0F;
  const float32_t u1 = 0.75F;
  const float32_t u2 = 0.25F;
  x1 = {-3.0F, 5.0F};
  x2 = {5.0F, -3.0F};
  x_mix = {-1.0F, 3.0F};
  ParameterEstimator<dim> model;
  Esrcf<dim, dim> kf(model, GQ, x1, C);
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::start();
  // P1' = u P1 * (x1 - x) (x1 - x)^T
  kf.imm_self_mix(u1, x_mix);
  EXPECT_FLOAT_EQ(model.get_state()(0U), -1.0F);
  EXPECT_FLOAT_EQ(model.get_state()(1U), 3.0F);
  P = P1;
  // dx = {-2, 2} --> dx * dx' = {4, -4; -4, 4}
  P += (Matrix<float32_t, dim, dim>() << 4.0F, -4.0F, -4.0F, 4.0F).finished();
  P *= u1;
  C = P;
  llt.compute(C);
  C(0U, 1U) = 0.0F;
  EXPECT_FLOAT_EQ(kf.get_covariance()(0U, 0U), C(0U, 0U));
  EXPECT_LT(fabsf(kf.get_covariance()(0U, 1U) - C(0U, 1U)), TOL);
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 0U), C(1U, 0U));
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 1U), C(1U, 1U));
  // Apply update from other model
  C = P2;
  llt.compute(C);

  C(0U, 1U) = 0.0F;
  kf.imm_other_mix(u2, x2, C, dim);
  // inputs should be zero'd after triangularization
  EXPECT_LT(fabsf(C(0U, 0U)), TOL);
  EXPECT_LT(fabsf(C(0U, 1U)), TOL);
  EXPECT_LT(fabsf(C(1U, 0U)), TOL);
  EXPECT_LT(fabsf(C(1U, 1U)), TOL);
  // internal state should remain unchanged
  EXPECT_FLOAT_EQ(model.get_state()(0U), -1.0F);
  EXPECT_FLOAT_EQ(model.get_state()(1U), 3.0F);
  // Compute updated covariance
  // dx2 = {6, -6}
  P2 += (Matrix<float32_t, dim, dim>() << 36.0F, -36.0F, -36.0F, 36.0F).finished();
  P2 *= u2;
  P += P2;
  C = P;
  C(0U, 1U) = 0.0F;
  llt.compute(C);

  EXPECT_FLOAT_EQ(kf.get_covariance()(0U, 0U), C(0U, 0U));
  EXPECT_LT(fabsf(kf.get_covariance()(0U, 1U) - C(0U, 1U)), TOL);
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 0U), C(1U, 0U));
  EXPECT_FLOAT_EQ(kf.get_covariance()(1U, 1U), C(1U, 1U));
  // TODO(ltbj): implement memory_test after the completion of #39
  // osrf_testing_tools_cpp::memory_test::stop();
}
#endif  // TEST_KALMAN_FILTER_HPP_


#include <jni.h>
#include "processor_MatrixProcessor.h"
#include <stdio.h>
#include <Eigen/SparseCore>

#include <vector>

typedef Eigen::SparseMatrix<float, Eigen::RowMajor> SpMatf;
typedef Eigen::Triplet<float> Tf;

struct CellInfo {
	int row;
	int col;
	float value;
};

// Código implementado externamente via CUDA.
extern "C" {

	void g_ResetAndSetGPUDevice(int gpuDevice);
	bool g_IsDeviceEnabled();
	int g_getDeviceCount();

	void g_MatMul(int n_rowsA, int n_colsA, int n_colsB, int nzA, int nzB, int *rowsA, int *colsA, float *valuesA,
			int *rowsB, int *colsB, float *valuesB, int **row_res, int **cols_res, float **values_res, int& res_nz);
	void g_Confidence(float* values, float* diagonal, int elements, float* result);
	void g_StandardDeviation(float* mat, int rows, int cols, float* meanSD, float* result);
	void g_MeanSD(int rows, int cols, int depth, float *h_data, float *result, bool considerZeros);

	void g_Sum(float* values1, float* values2, int elements, float*result);
	void g_Subtract(float* values1, float* values2, int elements, float*result);
	void g_Binarize(float* values, int elements, float* result);
	void g_Transpose(float* values, int elements, float* result);
	void g_Invert(float* values, int elements, float* result);
	void g_Diagonalize(float* values, int vertices, float* result);
	void g_UpperDiagonal(float* values, int vertices, float* result);
	void g_LowerDiagonal(float* values, int vertices, float* result);
	void g_TransitiveClosure(float* values, int vertices, float* result);
}

void deleteMatrix(SpMatf *matrix){
	delete matrix;
	matrix = NULL;
}

void setNonZeroData(SpMatf* mat, int* rows, int* cols, float* values, int size) {
	std::vector<Tf> tripletList;
	for (int i = 0; i < size; i++) {
		tripletList.push_back(Tf(rows[i], cols[i], values[i]));
	}
	mat->setFromTriplets(tripletList.begin(), tripletList.end());
}

void matrixMult(SpMatf *mat1, SpMatf *mat2, SpMatf *result){
	int nonZeros_1 = mat1->nonZeros();
	int nonZeros_2 = mat2->nonZeros();

	int *rows_1 = (int*) malloc(sizeof(int) * nonZeros_1);
	int *cols_1 = (int*) malloc(sizeof(int) * nonZeros_1);
	int *rows_2 = (int*) malloc(sizeof(int) * nonZeros_2);
	int *cols_2 = (int*) malloc(sizeof(int) * nonZeros_2);
	float *values_1 = (float*) malloc(sizeof(float) * nonZeros_1);
	float *values_2 = (float*) malloc(sizeof(float) * nonZeros_2);

	int k = 0;
	for (int i = 0; i < mat1->outerSize(); ++i){
		for (SpMatf::InnerIterator it((*mat1), i); it; ++it){
			rows_1[k] = it.row();
			cols_1[k] = it.col();
			values_1[k] = it.value();
			k++;
		}
	}

	k = 0;
	for (int i = 0; i < mat2->outerSize(); ++i){
		for (SpMatf::InnerIterator it((*mat2), i); it; ++it){
			rows_2[k] = it.row();
			cols_2[k] = it.col();
			values_2[k] = it.value();
			k++;
		}
	}

	int *res_rows, *res_cols, res_nz;
	float *res_data;
	g_MatMul(mat1->rows(), mat1->cols(), mat2->cols(), nonZeros_1, nonZeros_2,
			rows_1, cols_1, values_1, rows_2, cols_2, values_2, &res_rows, &res_cols, &res_data, res_nz);
	setNonZeroData(result, res_rows, res_cols, res_data, res_nz);
	free(res_rows);
	free(res_cols);
	free(res_data);
	free(rows_1);
	free(cols_1);
	free(rows_2);
	free(cols_2);
	free(values_1);
	free(values_2);
}

void calculateConfidence(SpMatf *mat1, SpMatf *result){
	int nonZeros = mat1->nonZeros();

	int *rows = (int*) malloc(sizeof(int) * nonZeros);
	int *cols = (int*) malloc(sizeof(int) * nonZeros);
	float *values = (float*) malloc(sizeof(float) * nonZeros);
	float *diagonal = (float*) malloc(sizeof(float) * nonZeros);
	float *res_data = (float*) malloc(sizeof(float) * nonZeros);

	int k = 0;
	for (int i = 0; i < mat1->outerSize(); ++i){
		for (SpMatf::InnerIterator it((*mat1), i); it; ++it){
			rows[k] = it.row();
			cols[k] = it.col();
			values[k] = it.value();
			diagonal[k] = mat1->coeffRef(rows[k], rows[k]);
			k++;
		}
	}
	g_Confidence(values, diagonal, nonZeros, res_data);
	setNonZeroData(result, rows, cols, res_data, nonZeros);
	free(rows);
	free(cols);
	free(res_data);
	free(values);
	free(diagonal);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    resetGPU
 * Signature: (I)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_resetGPU
  (JNIEnv *env, jclass obj, jint gpuDevice) {
	g_ResetAndSetGPUDevice(gpuDevice);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    isGPUEnabled
 * Signature: ()Z
 */
JNIEXPORT jboolean JNICALL Java_processor_MatrixProcessor_isGPUEnabled
  (JNIEnv *env, jclass obj) {
	return g_IsDeviceEnabled();
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    getDeviceCount
 * Signature: ()I
 */
JNIEXPORT jint JNICALL Java_processor_MatrixProcessor_getDeviceCount
  (JNIEnv *env, jclass obj) {
	return g_getDeviceCount();
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    getSparseData
 * Signature: (J)[Lprocessor/Cell;
 */
JNIEXPORT jobjectArray JNICALL Java_processor_MatrixProcessor_getSparseData
  (JNIEnv *env, jclass obj, jlong pointer) {
	SpMatf* mat = (SpMatf*)pointer;

	std::vector<CellInfo> nonZeros;

	for (int i = 0; i < mat->outerSize(); ++i) {
		for (SpMatf::InnerIterator it((*mat), i); it; ++it) {
			CellInfo nz;
			nz.row = it.row();
			nz.col = it.col();
			nz.value = it.value();
			nonZeros.push_back(nz);
		}
	}

	jclass cell_class = env->FindClass("processor/Cell");
	jmethodID defConstructor = env->GetMethodID(cell_class, "<init>", "()V");
	jfieldID cell_row = env->GetFieldID(cell_class, "row", "I");
	jfieldID cell_col = env->GetFieldID(cell_class, "col", "I");
	jfieldID cell_value = env->GetFieldID(cell_class, "value", "F");

	jobjectArray jNonZeroArray = env->NewObjectArray(nonZeros.size(),
		cell_class, NULL);

	for (int i = 0; i < nonZeros.size(); i++) {

		jobject obj = env->NewObject(cell_class, defConstructor);

		env->SetIntField(obj, cell_row, nonZeros[i].row);
		env->SetIntField(obj, cell_col, nonZeros[i].col);
		env->SetFloatField(obj, cell_value, nonZeros[i].value);

		env->SetObjectArrayElement(jNonZeroArray, i, obj);
	}

	return jNonZeroArray;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    getDenseData
 * Signature: (J[I[I)[Lprocessor/Cell;
 */
JNIEXPORT jobjectArray JNICALL Java_processor_MatrixProcessor_getDenseData
  (JNIEnv *env, jclass obj, jlong pointer, jintArray rows, jintArray cols) {
	float* valuesData = (float*)pointer;

	jsize cellSize = env->GetArrayLength(rows);
	jint* _jRowsData = env->GetIntArrayElements(rows, NULL);
	jint* _jColsData = env->GetIntArrayElements(cols, NULL);

	int* _rowsData = (int*)malloc(sizeof(int) * cellSize);
	int* _colsData = (int*)malloc(sizeof(int) * cellSize);

	for (int i = 0; i < cellSize; i++) {
		_rowsData[i] = _jRowsData[i];
	}

	for (int i = 0; i < cellSize; i++) {
		_colsData[i] = _jColsData[i];
	}

	std::vector<CellInfo> cells;

	for (int i=0; i < cellSize; i++) {
		CellInfo cell;
		cell.row = _rowsData[i];
		cell.col = _colsData[i];
		cell.value = valuesData[i];
		cells.push_back(cell);
	}

	jclass cell_class = env->FindClass("processor/Cell");
	jmethodID defConstructor = env->GetMethodID(cell_class, "<init>", "()V");
	jfieldID cell_row = env->GetFieldID(cell_class, "row", "I");
	jfieldID cell_col = env->GetFieldID(cell_class, "col", "I");
	jfieldID cell_value = env->GetFieldID(cell_class, "value", "F");

	jobjectArray jArray = env->NewObjectArray(cells.size(), cell_class, NULL);

	for (int i = 0; i < cells.size(); i++) {
		jobject obj = env->NewObject(cell_class, defConstructor);
		env->SetIntField(obj, cell_row, cells[i].row);
		env->SetIntField(obj, cell_col, cells[i].col);
		env->SetFloatField(obj, cell_value, cells[i].value);
		env->SetObjectArrayElement(jArray, i, obj);
	}
	return jArray;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    createSparseMatrix
 * Signature: (IIZ)J
 */
JNIEXPORT jlong JNICALL Java_processor_MatrixProcessor_createSparseMatrix
  (JNIEnv *env, jclass obj, jint rows, jint cols) {
	SpMatf *mat = new SpMatf(rows, cols);
	return (long) mat;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    createDenseMatrix
 * Signature: (IIZ)J
 */
JNIEXPORT jlong JNICALL Java_processor_MatrixProcessor_createDenseMatrix
  (JNIEnv *env, jclass obj, jint rows, jint cols) {
	float* v = (float*)malloc(sizeof(float) * rows*cols);
	return (long) v;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    deleteSparseMatrix
 * Signature: (J)Z
 */
JNIEXPORT jboolean JNICALL Java_processor_MatrixProcessor_deleteSparseMatrix
  (JNIEnv *env, jclass obj, jlong pointer) {
	SpMatf* _matrix = (SpMatf*) pointer;
	deleteMatrix(_matrix);
	return true;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    deleteDenseMatrix
 * Signature: (J)Z
 */
JNIEXPORT jboolean JNICALL Java_processor_MatrixProcessor_deleteDenseMatrix
  (JNIEnv *env, jclass obj, jlong pointer) {
	float* _matrix = (float*) pointer;
	delete _matrix;
	_matrix = NULL;
	return true;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    setSparseData
 * Signature: (J[I[I[F)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_setSparseData
  (JNIEnv *env, jclass obj, jlong pointer, jintArray rows, jintArray cols, jfloatArray values) {
	SpMatf* mat = (SpMatf*)pointer;

	jsize rowsSize = env->GetArrayLength(rows);
	jsize colsSize = env->GetArrayLength(cols);
	jint* _jRowsData = env->GetIntArrayElements(rows, NULL);
	jint* _jColsData = env->GetIntArrayElements(cols, NULL);
	jfloat* _valuesData = env->GetFloatArrayElements(values, NULL);

	int* _rowsData = (int*)malloc(sizeof(int) * rowsSize);
	int* _colsData = (int*)malloc(sizeof(int) * colsSize);

	for (int i = 0; i < rowsSize; i++) {
		_rowsData[i] = _jRowsData[i];
	}

	for (int i = 0; i < colsSize; i++) {
		_colsData[i] = _jColsData[i];
	}

	setNonZeroData(mat, _rowsData, _colsData, _valuesData, rowsSize);

	env->ReleaseIntArrayElements(rows, _jRowsData, 0);
	env->ReleaseIntArrayElements(cols, _jColsData, 0);
	env->ReleaseFloatArrayElements(values, _valuesData, 0);
	env->DeleteLocalRef(rows);
	env->DeleteLocalRef(cols);
	env->DeleteLocalRef(values);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    setDenseData
 * Signature: (J[F)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_setDenseData
  (JNIEnv *env, jclass obj, jlong pointer, jfloatArray jvalues) {
	float* values = (float*) pointer;
	jsize nzSize = env->GetArrayLength(jvalues);
	jfloat* _valuesData = env->GetFloatArrayElements(jvalues, NULL);

	for (int i = 0; i < nzSize; i++) {
		values[i]=_valuesData[i];
	}

	env->ReleaseFloatArrayElements(jvalues, _valuesData, 0);
	env->DeleteLocalRef(jvalues);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    multiply
 * Signature: (JJJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_multiply
  (JNIEnv *env, jclass obj, jlong mat1, jlong mat2, jlong result) {
	SpMatf* _matrix1 = (SpMatf*) mat1;
	SpMatf* _matrix2 = (SpMatf*) mat2;
	SpMatf* _matResult = (SpMatf*) result;

	matrixMult(_matrix1, _matrix2, _matResult);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    confidence
 * Signature: (JJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_confidence
  (JNIEnv *env, jclass obj, jlong pointer, jlong result) {
	SpMatf* _matrix = (SpMatf*) pointer;
	SpMatf* _res = (SpMatf*) result;

	calculateConfidence(_matrix, _res);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    getMin
 * Signature: (J)F
 */
JNIEXPORT jfloat JNICALL Java_processor_MatrixProcessor_getMin
  (JNIEnv *env, jclass obj, jlong pointer) {
	SpMatf* _matrix = (SpMatf*) pointer;
	float min = 0;
	for (int i = 0; i < _matrix->outerSize(); i++){
		for (SpMatf::InnerIterator it((*_matrix), i); it; ++it){
			if (it.value() < min)
				min = it.value();
		}
	}
	return min;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    getMax
 * Signature: (J)F
 */
JNIEXPORT jfloat JNICALL Java_processor_MatrixProcessor_getMax
  (JNIEnv *env, jclass obj, jlong pointer) {
	SpMatf* _matrix = (SpMatf*) pointer;
	float max = 0;
	for (int i = 0; i < _matrix->outerSize(); i++){
		for (SpMatf::InnerIterator it((*_matrix), i); it; ++it){
			if (it.value() > max)
				max = it.value();
		}
	}
	return max;
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    sum
 * Signature: (JJIJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_sum
  (JNIEnv *env, jclass obj, jlong pointer1, jlong pointer2, jint elements, jlong jresult) {
	float* values1 = (float*) pointer1;
	float* values2 = (float*) pointer2;
	float* result = (float*) jresult;
	g_Sum(values1, values2, elements, result);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    subtract
 * Signature: (JJIJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_subtract
  (JNIEnv *env, jclass obj, jlong pointer1, jlong pointer2, jint elements, jlong jresult) {
	float* values1 = (float*) pointer1;
	float* values2 = (float*) pointer2;
	float* result = (float*) jresult;
	g_Subtract(values1, values2, elements, result);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    binarize
 * Signature: (JJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_binarize
  (JNIEnv* env, jclass obj, jlong pointer, jlong result) {

	SpMatf* _mat = (SpMatf*)pointer;
	SpMatf* _res = (SpMatf*)result;

	int nonZeros = _mat->nonZeros();

	int* rows = (int*)malloc(sizeof(int) * nonZeros);
	int* cols = (int*)malloc(sizeof(int) * nonZeros);
	float* values = (float*)malloc(sizeof(float) * nonZeros);
	float* res_data = (float*)malloc(sizeof(float) * nonZeros);

	int k = 0;
	for (int i = 0; i < _mat->outerSize(); ++i) {
		for (SpMatf::InnerIterator it((*_mat), i); it; ++it) {
			rows[k] = it.row();
			cols[k] = it.col();
			values[k] = it.value();
			k++;
		}
	}

	g_Binarize(values, nonZeros, res_data);
	setNonZeroData(_res, rows, cols, res_data, nonZeros);

	free(rows);
	free(cols);
	free(res_data);
	free(values);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    transpose
 * Signature: (JJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_transpose
  (JNIEnv* env, jclass obj, jlong pointer, jlong result) {

	SpMatf* _mat = (SpMatf*)pointer;
		SpMatf* _res = (SpMatf*)result;

		int nonZeros = _mat->nonZeros();

		int* rows = (int*)malloc(sizeof(int) * nonZeros);
		int* cols = (int*)malloc(sizeof(int) * nonZeros);
		float* values = (float*)malloc(sizeof(float) * nonZeros);
		float* res_data = (float*)malloc(sizeof(float) * nonZeros);

		int k = 0;
		for (int i = 0; i < _mat->outerSize(); ++i) {
			for (SpMatf::InnerIterator it((*_mat), i); it; ++it) {
				rows[k] = it.row();
				cols[k] = it.col();
				values[k] = it.value();
				k++;
			}
		}

		g_Transpose(values, nonZeros, res_data);
		setNonZeroData(_res, cols, rows, res_data, nonZeros);

		free(rows);
		free(cols);
		free(res_data);
		free(values);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    invert
 * Signature: (IJJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_invert
  (JNIEnv *env, jclass obj, jint elements, jlong pointer, jlong jresult) {
	float* values = (float*) pointer;
	float* result = (float*) jresult;
	g_Invert(values, elements, result);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    diagonalize
 * Signature: (IJJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_diagonalize
  (JNIEnv *env, jclass obj, jint vertices, jlong pointer, jlong jresult) {
	float* values = (float*) pointer;
	float* result = (float*) jresult;
	g_Diagonalize(values, vertices, result);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    upperDiagonal
 * Signature: (IIJJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_upperDiagonal
  (JNIEnv *env, jclass obj, jint vertices, jlong pointer, jlong jresult) {
	float* values = (float*) pointer;
	float* result = (float*) jresult;
	g_UpperDiagonal(values, vertices, result);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    lowerDiagonal
 * Signature: (IIJJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_lowerDiagonal
  (JNIEnv *env, jclass obj, jint vertices, jlong pointer, jlong jresult) {
	float* values = (float*) pointer;
	float* result = (float*) jresult;
	g_LowerDiagonal(values, vertices, result);
}

/*
 * Class:     processor_MatrixProcessor
 * Method:    transitiveClosure
 * Signature: (IJJ)V
 */
JNIEXPORT void JNICALL Java_processor_MatrixProcessor_transitiveClosure
  (JNIEnv *env, jclass obj, jint vertices, jlong pointer, jlong jresult) {
	float* values = (float*) pointer;
	float* result = (float*) jresult;
	g_TransitiveClosure(values, vertices, result);
}


#ifndef BURST__CONTAINER__K_ARY_SEARCH_SET_HPP
#define BURST__CONTAINER__K_ARY_SEARCH_SET_HPP

#include <burst/container/unique_ordered_tag.hpp>
#include <burst/functional/not_fn.hpp>
#include <burst/integer/intlog.hpp>
#include <burst/integer/intpow.hpp>

#include <boost/assert.hpp>
#include <boost/container/container_fwd.hpp>
#include <boost/range/iterator_range.hpp>

#include <algorithm>
#include <cstddef>
#include <functional>
#include <initializer_list>
#include <iterator>
#include <stack>
#include <utility>
#include <vector>

namespace burst
{
    struct k_ary_search_set_branch
    {
        std::size_t index;
        std::size_t size;
        std::size_t height;
        std::size_t preceding_elements;
    };

    //!     Множество, основанное на k-местном дереве поиска.
    /*!
            k-местное дерево поиска — это дерево поиска, у которого в каждом узле находится не
        более (k - 1) элементов и не более k переходов в поддеревья.
            В данном случае используется правильное k-местное дерево, то есть такое, в котором
        обязательно заполнены все уровни дерева, кроме последнего, причём в последнем уровне
        элементы прибиты к левому краю уровня.
            При этом, естественно, соблюдается, условие дерева поиска, то есть все элементы,
        находящиеся в i-м поддереве узла, строго меньше i-го элемента узла и не меньше (i - 1)-го
        элемента узла, а i-й элемент узла не больше (i + 1)-го элемента узла.
            Повторяющихся элементов в дереве нет, это не мультимножество.
            Само дерево представлено в виде массива, элементы в котором размещены по принципу,
        схожему с пирамидой.

        \tparam Value
            Тип данных, хранящихся в множестве.
        \tparam Compare
            Отношение порядка, по которому элементы выстроены в дереве.
            Должно быть двухместной операцией на элементах типа Value, возвращающей булевское
            значение.

                bool Compare (Value [const &], Value [const &])
     */
    template<typename Value, typename Compare = std::less<>>
    class k_ary_search_set
    {
    public:
        using value_type = Value;
        using value_compare = Compare;

    private:
        using value_container_type = std::vector<value_type>;

    public:
        using iterator = typename value_container_type::iterator;
        using const_iterator = typename value_container_type::const_iterator;
        using size_type = typename value_container_type::size_type;
        using difference_type = typename value_container_type::difference_type;

    public:
        //!     Создание множества из упорядоченного набора, представленного итераторами.
        /*!
                Принимает два итератора, которые задают набор элементов, которые должны быть в
            результирующем множестве, местность дерева, отношение порядка на элементах, а также
            фиктивный элемент — метку, обозначающую, что входной набор элементов уже упорядочен и
            элементы в нём уникальны, то есть проводить дополнительную предобработку не нужно.

                Асимптотика.

            Время: O(N), N = |[first, last)| — размер дерева.
            Память: O(log_k(N) + k).
                Учитывается только собственная память конструктора. Созданное дерево, естественно,
                содержит ровно N элементов.
         */
        template <typename RandomAccessIterator>
        k_ary_search_set
                (
                    container::unique_ordered_tag_t,
                    RandomAccessIterator first,
                    RandomAccessIterator last,
                    std::size_t arity = default_arity,
                    const value_compare & compare = value_compare()
                ):
            m_values(static_cast<size_type>(std::distance(first, last))),
            m_arity(arity),
            m_compare(compare)
        {
            initialize_trusted(boost::make_iterator_range(first, last));
        }

        //!     Создание множества из набора, заданного итераторами.
        /*!
                Принимает два итератора, которые задают набор элементов, которые должны быть в
            результирующем множестве, местность дерева и отношение порядка на элементах.

                Асимптотика.

            Время:
                1. O(N), если набор упорядочен.
                2. O(N logN), если набор неупорядочен,
                где N = |[first, last)|.
            Память: O(log_k(N) + k).
         */
        template <typename RandomAccessIterator>
        k_ary_search_set
                (
                    RandomAccessIterator first,
                    RandomAccessIterator last,
                    std::size_t arity = default_arity,
                    const value_compare & compare = value_compare()
                ):
            m_arity(arity),
            m_compare(compare)
        {
            initialize(boost::make_iterator_range(first, last));
        }

        //!     Создание множества из упорядоченного набора, представленного списком инициализации.
        /*!
                Принимает std::initializer_list, заполненный элементами которые должны быть в
            результирующем множестве, местность дерева, отношение порядка на элементах, а также
            фиктивный элемент — метку, обозначающую, что входной набор элементов уже упорядочен и
            элементы в нём уникальны, то есть проводить дополнительную предобработку не нужно.

                Асимптотика.

            Время: O(N), где N = |values|.
            Память: O(log_k(N) + k).
         */
        k_ary_search_set
                (
                    container::unique_ordered_tag_t,
                    std::initializer_list<value_type> values,
                    std::size_t arity = default_arity,
                    const value_compare & compare = value_compare()
                ):
            m_values(values.size()),
            m_arity(arity),
            m_compare(compare)
        {
            initialize_trusted(boost::make_iterator_range(values));
        }

        //!     Создание множества из набора, представленного списком инициализации.
        /*!
                Принимает std::initializer_list, заполненный элементами которые должны быть в
            результирующем множестве, местность дерева и отношение порядка на элементах.

                Асимптотика.

            Время:
                1. O(N), если набор упорядочен.
                2. O(N logN), если набор неупорядочен,
                где N = |values|.
            Память: O(log_k(N) + k).
         */
        explicit k_ary_search_set
                (
                    std::initializer_list<value_type> values,
                    std::size_t arity = default_arity,
                    const value_compare & compare = value_compare()
                ):
            m_arity(arity),
            m_compare(compare)
        {
            initialize(boost::make_iterator_range(values));
        }

        k_ary_search_set ():
            m_arity(0)
        {
        }

    public:
        //!     Поиск элемента в множестве.
        /*!
                Если искомый элемент существует в множестве, то возвращается итератор на него. Если
            не существует, то возвращается end().

                Асимптотика.

            Время: O(log_k(N)),
                k — местность дерева.
                N — количество элементов в дереве.
            Память: O(1).
         */
        iterator find (const value_type & value)
        {
            return begin() + std::distance(cbegin(), find_impl(value));
        }

        const_iterator find (const value_type & value) const
        {
            return find_impl(value);
        }

        size_type size () const
        {
            return m_values.size();
        }

        bool empty () const
        {
            return m_values.empty();
        }

        //!     Начало множества.
        /*!
                Важно, что последовательность [begin(), end()) неупорядочена.
         */
        iterator begin ()
        {
            return m_values.begin();
        }

        iterator end ()
        {
            return m_values.end();
        }

        const_iterator begin () const
        {
            return m_values.begin();
        }

        const_iterator end () const
        {
            return m_values.end();
        }

        const_iterator cbegin () const
        {
            return m_values.cbegin();
        }

        const_iterator cend () const
        {
            return m_values.cend();
        }

    private:
        const_iterator find_impl (const value_type & value) const
        {
            std::size_t node_index = 0;

            while (node_index < m_values.size())
            {
                const_iterator node_begin = begin() + static_cast<difference_type>(node_index);
                const_iterator node_end =
                    node_begin +
                    std::min
                    (
                        static_cast<difference_type>(m_arity - 1),
                        std::distance(node_begin, end())
                    );

                const_iterator search_result =
                    std::lower_bound(node_begin, node_end, value, m_compare);
                if (search_result != node_end && not m_compare(value, *search_result))
                {
                    return search_result;
                }
                else
                {
                    node_index = perfect_tree_child_index
                    (
                        m_arity,
                        node_index,
                        static_cast<std::size_t>(std::distance(node_begin, search_result))
                    );
                }
            }

            return end();
        }

        template <typename RandomAccessRange>
        void initialize (const RandomAccessRange & range)
        {
            const auto is_sorted_and_unique =
                std::adjacent_find(range.begin(), range.end(), burst::not_fn(m_compare)) == range.end();
            if (is_sorted_and_unique)
            {
                initialize_trusted(range);
            }
            else
            {
                value_container_type buffer(range.begin(), range.end());
                std::sort(buffer.begin(), buffer.end(), m_compare);
                buffer.erase
                (
                    std::unique(buffer.begin(), buffer.end(), burst::not_fn(m_compare)),
                    buffer.end()
                );

                initialize_trusted(boost::make_iterator_range(buffer));
            }
        }

        //!     Расстановка элементов по своим местам.
        /*!
                Обходит дерево по уровням и расставляет по местам в дереве элементы исходной
            последовательности.

                Асимптотика.

            Время: O(N), N — количество элементов в дереве.
            Память: O(log_k(N) + k),
                O(log_k(N)) памяти требуется для хранения стека при обходе узлов дерева.
                O(k) памяти требуется для хранения счётчиков при заполнении одного узла.
         */
        template <typename RandomAccessRange>
        void initialize_trusted (const RandomAccessRange & range)
        {
            BOOST_ASSERT
            (
                std::adjacent_find(range.begin(), range.end(), burst::not_fn(m_compare)) == range.end()
            );
            if (not range.empty())
            {
                m_values.resize(range.size());

                std::stack<k_ary_search_set_branch> branches;

                branches.push({0, size(), perfect_tree_height(m_arity, size()), 0});
                while (not branches.empty())
                {
                    const auto branch = branches.top();
                    branches.pop();

                    // Количество меньших элементов ветки для каждого элемента текущего узла.
                    std::vector<std::size_t> counters;
                    fill_counters(branch, counters);

                    fill_node(branch, counters, range);

                    if (counters[0] > 0)
                    {
                        branches.push
                        ({
                            perfect_tree_child_index(m_arity, branch.index, 0),
                            counters[0],
                            branch.height - 1,
                            branch.preceding_elements
                        });
                    }

                    for
                    (
                        std::size_t i = 1;
                        i < counters.size() && (counters[i] - counters[i - 1] - 1) > 0;
                        ++i
                    )
                    {
                        branches.push
                        ({
                            perfect_tree_child_index(m_arity, branch.index, i),
                            counters[i] - counters[i - 1] - 1,
                            branch.height - 1,
                            branch.preceding_elements + counters[i - 1] + 1
                        });
                    }
                }
            }
        }

        //!     Подсчёт счётчиков для элементов узла.
        /*!
                Для каждого элемента узла подсчитывает количество элементов в ветке (ветка включает
            рассматриваемый узел), которые строго меньше этого элемента.
         */
        void
            fill_counters
            (
                const k_ary_search_set_branch & branch,
                std::vector<std::size_t> & counters
            )
        {
            const std::size_t max_subtree_height = branch.height - 1;
            const std::size_t min_subtree_elements =
                perfect_tree_size(m_arity, max_subtree_height - 1);
            const std::size_t max_subtree_elements =
                perfect_tree_size(m_arity, max_subtree_height);
            const std::size_t elements_in_last_row =
                branch.size - perfect_tree_size(m_arity, branch.height - 1);

            counters.resize(std::min(m_arity, branch.size + 1));
            for (std::size_t i = 0; i < counters.size(); ++i)
            {
                counters[i] = i + std::min
                (
                    (i + 1) * min_subtree_elements + elements_in_last_row,
                    (i + 1) * max_subtree_elements
                );
            }
            BOOST_ASSERT(counters.back() == branch.size);
        }

        //!     Заполнение узла нужными элементами исходного диапазона.
        /*!
                Для каждого элемента узла известен индекс в исходном диапазоне, по которому лежит
            нужное значение. Осталось только скопировать его.
         */
        template <typename RandomAccessRange>
        void
            fill_node
            (
                const k_ary_search_set_branch & branch,
                const std::vector<std::size_t> & counters,
                const RandomAccessRange & range
            )
        {
            for
            (
                std::size_t element_index = 0;
                element_index < counters.size() - 1;
                ++element_index
            )
            {
                using difference_type = typename RandomAccessRange::difference_type;
                const auto index_in_initial_range =
                    static_cast<difference_type>
                    (
                        branch.preceding_elements + counters[element_index]
                    );
                m_values[branch.index + element_index] = range[index_in_initial_range];
            }
            BOOST_ASSERT(std::is_sorted
            (
                m_values.begin() + static_cast<difference_type>(branch.index),
                m_values.begin() + static_cast<difference_type>(branch.index + counters.size() - 1),
                m_compare
            ));
        }

    private:
        static std::size_t perfect_tree_size (std::size_t arity, std::size_t height)
        {
            return intpow(arity, height) - 1;
        }

        static std::size_t perfect_tree_height (std::size_t arity, std::size_t size)
        {
            return intlog(size, arity) + 1;
        }

        static std::size_t
            perfect_tree_child_index
            (
                std::size_t arity,
                std::size_t parent_index,
                std::size_t child_number
            )
        {
            return parent_index * arity + (child_number + 1) * (arity - 1);
        }

    private:
        static const std::size_t default_arity = 33;

    private:
        value_container_type m_values;
        const std::size_t m_arity;
        value_compare m_compare;
    };
}

#endif // BURST__CONTAINER__K_ARY_SEARCH_SET_HPP


// Copyright (c) 2019 fortiss GmbH, Julian Bernhard, Klemens Esterle, Patrick
// Hart, Tobias Kessler
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.

#include <Eigen/Core>
#include "gtest/gtest.h"

#include "modules/commons/params/default_params.hpp"
#include "modules/commons/params/setter_params.hpp"
#include "modules/geometry/commons.hpp"
#include "modules/geometry/line.hpp"
#include "modules/geometry/polygon.hpp"
#include "modules/models/dynamic/integration.hpp"
#include "modules/models/dynamic/single_track.hpp"
#include "modules/models/dynamic/triple_integrator.hpp"

TEST(single_track_model, dynamic_test) {
  using namespace std;
  using namespace modules::geometry;
  using namespace modules::models::dynamic;
  using namespace modules::commons;

  State x(static_cast<int>(StateDefinition::MIN_STATE_SIZE));
  x << 0, 0, 0, 0, 5;

  Input u(2);
  u << 0, 0;

  DynamicModel *m;
  auto params = std::make_shared<DefaultParams>();
  SingleTrackModel single_track_model(params);
  m = &single_track_model;

  float dt = 0.1;
  for (int i = 0; i < 10; i++) {
    x = euler_int(*m, x, u, dt);
    cout << x << endl;
  }
}

TEST(CalculateSteeringAngle, dynamic_test) {
  using namespace std;
  using namespace modules::geometry;
  using namespace modules::models::dynamic;
  using namespace modules::commons;

  const float dt = 1.0;
  auto a_lat = [dt](const State &x, const State &x1) {
    auto theta_dot = (x1(static_cast<int>(StateDefinition::THETA_POSITION)) -
                      x(static_cast<int>(StateDefinition::THETA_POSITION))) /
                     dt;
    return theta_dot * x(static_cast<int>(StateDefinition::VEL_POSITION));
  };

  auto params = std::make_shared<DefaultParams>();
  DynamicModel *m;
  SingleTrackModelPtr single_track_model =
      std::make_shared<SingleTrackModel>(params);
  m = single_track_model.get();
  const float a_lat_max = single_track_model->GetLatAccelerationMax();
  const float delta_max = single_track_model->GetSteeringAngleMax();

  State x(static_cast<int>(StateDefinition::MIN_STATE_SIZE));

  Input u(2);

  Point2d point_1(0.0, 0.0);
  Point2d point_2(0.0, 10.0);

  Line_t<Point2d> line;

  line.AddPoint(point_1);
  line.AddPoint(point_2);

  // Parallel to line, high crosstrack error, high speed
  x << 50.0f, 0.0f, 0.0f, M_PI / 2.0f, 50.0f;
  auto delta = CalculateSteeringAngle(single_track_model, x, line, 1.0);
  u << 0.0f, delta;
  auto x1 = euler_int(*m, x, u, dt);

  EXPECT_LE(std::abs(a_lat(x, x1)), a_lat_max);
  EXPECT_LE(std::abs(delta), delta_max);
  EXPECT_LT(x1(static_cast<int>(StateDefinition::X_POSITION)),
            x(static_cast<int>(StateDefinition::X_POSITION)));

  // Parallel to line, high crosstrack error, low speed
  x << 50.0f, 0.0f, 0.0f, M_PI / 2.0f, 0.1f;
  delta = CalculateSteeringAngle(single_track_model, x, line, 1.0);
  u << 0.0f, delta;
  x1 = euler_int(*m, x, u, dt);

  EXPECT_LE(std::abs(a_lat(x, x1)), a_lat_max);
  EXPECT_LE(std::abs(delta), delta_max);
  EXPECT_LT(x1(static_cast<int>(StateDefinition::X_POSITION)),
            x(static_cast<int>(StateDefinition::X_POSITION)));
}

TEST(triple_integrator_model, dynamic_test) {
  using namespace std;
  using namespace modules::geometry;
  using namespace modules::models::dynamic;
  using namespace modules::commons;

  State x(15);
  x << 0, 0, 0, 0, 0, 0,  // time, x, y, theta, v, min_space
      0, 1, 0,            // x, vx, ax
      0, 1, 0,            // y, vy, ay
      0, 1, 0;            // z, vz, az

  Input u0(3);
  u0 << 0, 0, 0.;

  DynamicModel *m;
  auto params = std::make_shared<DefaultParams>();
  TripleIntegratorModel triple_int_model(params);
  m = &triple_int_model;

  float dt = 0.1;
  for (int i = 0; i < 10; i++) {
    x = euler_int(*m, x, u0, dt);
    cout << x << endl << endl;
  }
  // TODO(@hart): assert state
  u0 << 1., 1., 1.;
  for (int i = 0; i < 10; i++) {
    x = euler_int(*m, x, u0, dt);
    cout << x << endl << endl;
  }
  // TODO(@hart): assert state
}

int main(int argc, char **argv) {
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}