/* Copyright (c) 2015, Julian Straub <jstraub@csail.mit.edu>
 * Licensed under the MIT license. See the license file LICENSE.
 */

#pragma once

#include <vector>
#include <Eigen/Dense>

#include <dpMM/gpuMatrix.hpp>
#include <dpMM/clGMMData.hpp>
#include <dpMM/timer.hpp>

using namespace Eigen;
using std::vector;

extern void sufficientStatistics_gpu(double *d_x, uint32_t *d_z , uint32_t N,
    uint32_t k0, uint32_t K, double *d_SSs);
extern void gmmPdf(double * d_x, double *d_invSigmas,
    double *d_logNormalizers, double *d_logPi, double* d_logPdf, uint32_t N,
    uint32_t K_);

extern void sufficientStatistics_gpu(float *d_x, uint32_t *d_z , uint32_t N,
    uint32_t k0, uint32_t K, float *d_SSs);
extern void gmmPdf(float * d_x, float *d_invSigmas,
    float *d_logNormalizers, float *d_logPi, float* d_logPdf, uint32_t N,
    uint32_t K_);

template<typename T>
class ClGMMDataGpu : public ClGMMData<T>
{
protected:
  GpuMatrix<uint32_t> d_z_; // indicators on GPU
  GpuMatrix<T> d_x_; // data-points

  GpuMatrix<T> d_Ss_; // sufficient statistics

  shared_ptr<GpuMatrix<T> > d_pdfs_; //
  GpuMatrix<T> d_logPi_; //
  GpuMatrix<T> d_logNormalizers_; //
  GpuMatrix<T> d_invSigmas_; //

public:
  ClGMMDataGpu(const shared_ptr<Matrix<T,Dynamic,Dynamic> >& x,
      const spVectorXu& z, uint32_t K);
  virtual ~ClGMMDataGpu(){;};

  virtual void init();

  virtual void updateLabels(uint32_t K);

  virtual void computeSufficientStatistics();

  virtual void computeLogLikelihoods(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers);
  virtual void sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
      const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
      const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler);

  Matrix<T,Dynamic,Dynamic> pdfs(){ return d_pdfs_->get();};
  shared_ptr<GpuMatrix<T> >& d_pdfs(){ return d_pdfs_;};
  GpuMatrix<T>& d_x(){ return d_x_;};
  GpuMatrix<uint32_t>& d_z(){ return d_z_;};

protected:

  virtual void computeSufficientStatistics(uint32_t k0, uint32_t K);
};

typedef ClGMMDataGpu<double> ClGMMDataGpud;
typedef ClGMMDataGpu<float> ClGMMDataGpuf;

// ------------------------------------ impl --------------------------------
template<typename T>
ClGMMDataGpu<T>::ClGMMDataGpu(const shared_ptr<Matrix<T,Dynamic,Dynamic> >& x,
    const spVectorXu& z, uint32_t K)
  : ClGMMData<T>(x,z,K), d_z_(this->N_,1),
    d_x_(this->D_,this->N_),
    d_Ss_((this->D_-1)+(this->D_-1)*(this->D_-1)+1,this->K_),
    d_pdfs_(new GpuMatrix<T>(this->N_,this->K_)), d_logPi_(this->K_),
    d_logNormalizers_(this->K_),
    d_invSigmas_((this->D_-1)*(this->D_-1),this->K_)
{};

template<typename T>
void ClGMMDataGpu<T>::init()
{
  d_x_.set(*this->x_);
};

template<typename T>
void ClGMMDataGpu<T>::updateLabels(uint32_t K)
{
  this->K_ = K>0?K:this->z_->maxCoeff()+1;
  assert(this->z_->maxCoeff() < this->K_); // no indicators \geq K
  assert(this->z_->minCoeff() >= 0); // no indicators \le 0
  assert((this->z_->array() < this->K_).all());
  // update the labels on the GPU
  d_z_.set(this->z_);
}

template<typename T>
void ClGMMDataGpu<T>::computeSufficientStatistics(uint32_t k0, uint32_t K)
{
  // TODO: could probably move this up to bas class!
  assert(k0+K <= this->K_);
  assert(K<=6); //limitation of the GPU code - mainly of GPU shared mem
//  cout << d_x_.get() <<endl;

  Matrix<T,Dynamic,Dynamic> Ss = Matrix<T,Dynamic,Dynamic>::Zero(
      (this->D_-1)+(this->D_-1)*(this->D_-1)+1,K);
  d_Ss_.set(Ss);
  //  cout<<ps_.rows()<<"x"<<ps_.cols()<<endl;
//  cout<<"SS around"<<endl<<d_ps_.get()<<endl;
  // does reuse linearized datapoints
  sufficientStatistics_gpu(d_x_.data(), d_z_.data(), this->N_, k0, K,
      d_Ss_.data());
  // does compute linearization by itself
//  sufficientStatisticsOnTpS2_gpu(d_ps_.data(), d_northRps_.data(), d_q_.data(),
//      d_z_.data() , this->N_, k0, K, d_Ss_.data());
  d_Ss_.get(Ss);
  cout<<Ss<<endl;
  for (uint32_t k=0; k<K; ++k)
  {
    this->Ss_[k+k0](0,0) =  Ss(2,k);
    this->Ss_[k+k0](0,1) =  Ss(3,k);
    this->Ss_[k+k0](1,0) =  Ss(4,k);
    this->Ss_[k+k0](1,1) =  Ss(5,k);
    this->Ns_(k+k0) = Ss(6,k);
    if(this->Ns_(k+k0) > 0.)
      this->means_.col(k+k0) = Ss.block(0,k,2,1)/this->Ns_(k+k0);
    // SS on GPU is just sum of outer products of all data -> make it a scatter
    // matrix now!
    //    cout<<"S"<<endl<<this->Ss_[k]<<endl;;
    this->Ss_[k+k0] -= this->Ns_(k+k0)*this->means_.col(k+k0)*this->means_.col(k+k0).transpose();
    //    cout<<"S"<<endl<<this->Ss_[k]<<endl;;
  }
}

template<typename T>
void ClGMMDataGpu<T>::computeSufficientStatistics()
{
  uint32_t k0 = 0;
  for (k0=0; k0<this->K_; k0+=6)
  {
    computeSufficientStatistics(k0,6); // max 6 SSs per kernel due to shared mem
  }
  if(this->K_ - k0 > 0)
    computeSufficientStatistics(k0,this->K_-k0);
  //cout<<xSums_<<endl;
  //cout<<Ns_<<endl;
}

template<typename T>
void ClGMMDataGpu<T>::computeLogLikelihoods(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers)
{
//  cout<<"ClGMMDataGpu<T>::sampleGMMpdf"<<endl;
  assert(pi.size() == this->K_);
  assert(logNormalizers.size() == this->K_);

  Matrix<T,Dynamic,Dynamic> invSigmas((this->D_-1)*(this->D_-1),this->K_);
//  Matrix<T,Dynamic,1> logNormalizer(this->K_);
  for(uint32_t k=0; k<this->K_; ++k)
  {
    Matrix<T,Dynamic,Dynamic> invS =Sigmas[k].inverse();
    for(uint32_t i=0; i<invS.cols(); ++i)
      for(uint32_t j=0; j<invS.rows(); ++j)
        invSigmas(i*invS.rows()+j,k) = invS(j,i);
//    logNormalizer(k) = -0.5*Sigmas[k]
  }
  assert( fabs(pi.sum()-1.0) <1e-6);
  Matrix<T,Dynamic,1> logPi = pi.array().log().matrix();
  //copy parameters into memory
  this->d_logPi_.set(logPi);
  this->d_invSigmas_.set(invSigmas);
  this->d_logNormalizers_.set(logNormalizers);

//  cout<<"sphereGMMPdf"<<endl;
//  cout<<"logNormaloizers"<<endl<<logNormalizers.transpose()<<endl;
//  cout<<"logPi"<<endl<<logPi.transpose()<<endl;
//  cout<<invSigmas<<endl;
//
//  d_ps_.data();
//  d_northRps_.data();
//  d_q_.data();
//  d_invSigmas_.data();
//  d_logNormalizers_.data();
//  d_logPi_.data();

  Matrix<T,Dynamic,Dynamic> pdfs(this->N_,this->K_);
  if(!this->d_pdfs_->isInit())
  {
    this->d_pdfs_->setZero();
//    pdfs = Matrix<T,Dynamic,Dynamic>::Zero(this->N_,this->K_);
//    d_pdfs_.set(pdfs);
  }

  gmmPdf(d_x_.data(), d_invSigmas_.data(), d_logNormalizers_.data(),
    d_logPi_.data(), d_pdfs_->data(), this->N_, this->K_);

};

template<typename T>
void ClGMMDataGpu<T>::sampleGMMpdf(const Matrix<T,Dynamic,1>& pi,
    const vector<Matrix<T,Dynamic,Dynamic> >& Sigmas,
    const Matrix<T,Dynamic,1>& logNormalizers, Sampler<T> *sampler)
{

  computeLogLikelihoods(pi,Sigmas,logNormalizers);

  sampler->sampleDiscPdf(d_pdfs_->data(),this->z_);

//  cout<< this->z_->transpose()<<endl;
//  d_pdfs_.get(pdfs);
//  cout<<pdfs<<endl;

};


#include <Eigen/Core>

#include <numpy_eigen/boost_python_headers.hpp>
Eigen::Matrix<double, 5, 2> test_double_5_2(const Eigen::Matrix<double, 5, 2> & M)
{
	return M;
}
void export_double_5_2()
{
	boost::python::def("test_double_5_2",test_double_5_2);
}


/*  ==========================================================================================
    Author: Leonardo Citraro
    Company:
    Filename: wrapper.cpp
    Last modifed:   17.3.2021 by Leonardo Citraro
    Description:    Boost-Python wrappers
    ==========================================================================================
    Copyright (c) 2017 Leonardo Citraro <ldo.citraro@gmail.com>
    Permission is hereby granted, free of charge, to any person obtaining a copy of this
    software and associated documentation files (the "Software"), to deal in the Software
    without restriction, including without limitation the rights to use, copy, modify,
    merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
    permit persons to whom the Software is furnished to do so, subject to the following
    conditions:
    The above copyright notice and this permission notice shall be included in all copies
    or substantial portions of the Software.
    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
    INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
    PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
    FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
    OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
    DEALINGS IN THE SOFTWARE.
    ==========================================================================================
*/
#ifndef BOOST_PYTHON_STATIC_LIB
#define BOOST_PYTHON_STATIC_LIB
#endif
#include <Python.h>
#include <boost/python.hpp>
#include <algorithm>
#include <unordered_map>
#include "Graph.h"

using namespace std;
namespace p = boost::python;

inline size_t node_key(int i, int j) { return (size_t) i << 32 | (unsigned int) j; }

Graph* init(p::list graph_as_text, int verbose) {
    int n, m; ////no of nodes, no of arcs;
    char pr_type[3]; ////problem type;
    int tail, head;
    double weight=0;
    double en_weight=0;
    double ex_weight=0;

    vector<int> edge_tails, edge_heads;
    vector<double> edge_weights;

    p::ssize_t len = p::len(graph_as_text);

    string line_inf = p::extract<string>(graph_as_text[0]);
    sscanf(line_inf.c_str(), "%*c %3s %d %d", pr_type, &n, &m);

    auto *resG = new Graph(n, m, 0, n - 1, en_weight, ex_weight);
    int edges = 0;
    int edge_id = 0;
    if (verbose>=2)
        cout << "Parsing edges: " <<endl;

    string line;
    for (int i=1; i<len; i++) {
        line = p::extract<string>(graph_as_text[i]);
        switch (line[0]) {
            case 'c':                  /* skip lines with comments */
            case '\n':                 /* skip empty lines   */
            case 'n':
            case '\0':                 /* skip empty lines at the end of file */
                break;
            case 'p':
            case 'a': {
                sscanf(line.c_str(), "%*c %d %d %lf", &tail, &head, &weight);
                edges++;

                resG->add_edge(tail - 1, head - 1, edge_id, weight);
                edge_id++;
                if (edges % 10000 == 0)
                    cout << edges << endl;
                break;
            }
            default:
                break;
        }
    }
    if (verbose>=2)
        cout <<"Parsing done!"<<endl;
    return resG;
}
/* there is still an issue with this
p::list make_output(Graph* resG, vector<vector<int>> path_set, double cost_sum) {
    //r some reason path_set doesn't contain the actual (final) shortest paths.
    //It contains the information of which edge is active in the graph instead.
    //This function outputs these edges.
    //!! some edges may be flipped !!
    //The next step would be to extract the subgraph composed of only active edges and then compute all paths
    //connecting source to sink. (this is done in the python code)

    int i, j;
    int tail, head, edge_id;
    std::unordered_map<size_t, int> m;

    std::vector<bool> edge_visited;

    for (i = 0; i < path_set.size(); i++) {
        for (j = 0; j < path_set[i].size() - 1; j++) {
            head = path_set[i][j];
            tail = path_set[i][j + 1];

            if (m.find(node_key(tail, head)) == m.end()) {
                // not found condition
                edge_visited.push_back(true);
                m.insert({node_key(tail, head), edge_visited.size()-1});
            }else{
                edge_id = m[node_key(tail, head)];
                edge_visited[edge_id] = !edge_visited[edge_id];
            }
        }
    }

    p::list edges;
    for (i = 0; i < path_set.size(); i++) {
        for (j = path_set[i].size() - 1; j>0; j--) {
            //head = path_set[i][j];
            //tail = path_set[i][j - 1];
            head = path_set[i][j-1];
            tail = path_set[i][j];

            edge_id = m[node_key(tail, head)];
            if (edge_visited[edge_id]) {
                //edges.append(p::make_tuple(tail+1, head+1));
                edges.append(p::make_tuple(head+1, tail+1));
            }
        }
    }

    return edges;
}
*/
p::list make_output2(Graph* resG, vector<vector<int>> path_set, double cost_sum) {
    // this requires that the flag in muSSP-master/muSSP/Graph.cpp Graph::add_edge is set to true!

    int i, j;
    int tail, head, edge_id;

    std::vector<bool> edge_visited_flag(resG->num_edges_);

    for (i = 0; i < path_set.size(); i++) {
        for (j = 0; j < path_set[i].size() - 1; j++) {
            head = path_set[i][j];
            tail = path_set[i][j + 1];

            int edge_idx = resG->node_id2edge_id[Graph::node_key(tail, head)];
            edge_visited_flag[edge_idx] = !edge_visited_flag[edge_idx];
        }
    }

    p::list edges;
    for (int i = 0; i < resG->num_edges_; i++) {
        if (edge_visited_flag[i]){

            head = resG->edge_tail_head[i].first;
            tail = resG->edge_tail_head[i].second;

            edges.append(p::make_tuple(head+1, tail+1));
        }
    }

    return edges;
}

p::list solve(p::list graph_as_text, int verbose) {

    clock_t t_start;
    clock_t t_end;

    //// reading data
    t_start = clock();

    std::unique_ptr<Graph> org_graph = std::unique_ptr<Graph>(init(graph_as_text, verbose));

    t_end = clock();
    long double parsing_time = t_end - t_start;

    auto *duration = new long double[10];
    for (int i = 0; i < 10; i++)
        duration[i] = 0;

    //// 1: remove dummy edges
    t_start = clock();
    org_graph->invalid_edge_rm();
    t_end = clock();
    duration[0] = t_end - t_start;

    ////save path and path cost
    vector<double> path_cost;
    vector<vector<int>> path_set;
    int path_num = 0;

    t_start = clock();
    //// 2: initialize shortest path tree from the DAG
    org_graph->shortest_path_dag();
    t_end = clock();
    duration[1] = duration[1] + t_end - t_start;

    path_cost.push_back(org_graph->distance2src[org_graph->sink_id_]);
    org_graph->cur_path_max_cost = -org_graph->distance2src[org_graph->sink_id_]; // the largest cost we can accept

    //// 3: convert edge cost (make all weights positive)
    t_start = clock();
    org_graph->update_allgraph_weights();
    t_end = clock();
    duration[2] = duration[2] + t_end - t_start;

    //// 8: extract shortest path
    t_start = clock();
    org_graph->extract_shortest_path();
    t_end = clock();
    duration[7] = duration[7] + t_end - t_start;

    path_set.push_back(org_graph->shortest_path);
    path_num++;

    vector<unsigned long> update_node_num;

    //// 4: find nodes for updating based on branch node
    vector<int> node_id4updating;
    t_start = clock();
    org_graph->find_node_set4update(node_id4updating);
    t_end = clock();
    duration[3] = duration[3] + t_end - t_start;

    //// 10: rebuild residual graph by flipping paths
    t_start = clock();
    org_graph->flip_path();//also erase the top sinker
    t_end = clock();
    duration[9] = duration[9] + t_end - t_start;

    while (true) {

        //// 6: update shortest path tree based on the selected sub-graph
        t_start = clock();
        org_graph->update_shortest_path_tree_recursive(node_id4updating);
        if (verbose>=2)
            printf("Iteration #%d, updated node number  %ld \n", path_num, org_graph->upt_node_num);
        t_end = clock();
        duration[5] = duration[5] + t_end - t_start;

        //// 7: update sink node (heap)
        t_start = clock();
        org_graph->update_sink_info(node_id4updating);
        t_end = clock();
        duration[6] = duration[6] + t_end - t_start;

        update_node_num.push_back(node_id4updating.size());

        //// 8: extract shortest path
        t_start = clock();
        org_graph->extract_shortest_path();
        t_end = clock();
        duration[7] = duration[7] + t_end - t_start;

        //// test if stop
        double cur_path_cost = path_cost[path_num - 1] + org_graph->distance2src[org_graph->sink_id_];

        if (cur_path_cost > -0.0000001) {
            break;
        }

        path_cost.push_back(cur_path_cost);
        org_graph->cur_path_max_cost = -cur_path_cost;
        path_set.push_back(org_graph->shortest_path);
        path_num++;

        //// 9: update weights
        t_start = clock();
        org_graph->update_subgraph_weights(node_id4updating);
        t_end = clock();
        duration[8] = duration[8] + t_end - t_start;

        //// 4: find nodes for updating
        t_start = clock();
        org_graph->find_node_set4update(node_id4updating);
        t_end = clock();
        duration[3] = duration[3] + t_end - t_start;
        //// 10: rebuild the graph
        t_start = clock();
        org_graph->flip_path();
        t_end = clock();
        duration[9] = duration[9] + t_end - t_start;
    }

    //// out put results and time consuming
    if (verbose>=2)
        cout << "Parsing time is: " << parsing_time / CLOCKS_PER_SEC << " s" << endl;

    long double all_cpu_time = 0;
    for (int i = 0; i < 10; i++) {
        auto time_elapsed_ms = 1000.0 * duration[i] / CLOCKS_PER_SEC;
        all_cpu_time += time_elapsed_ms;
        if (verbose>=2)
            cout << "the " << i + 1 << " step used: " << time_elapsed_ms / 1000.0 << " s\n";
    }
    if (verbose>=1)
        cout << "The overall time is " << all_cpu_time / 1000.0 << " s\n\n";

    double cost_sum = 0;
    for (int i = 0; i < path_cost.size(); i++) {
        cost_sum += path_cost[i];
        if (verbose>=1)
            printf("cost path #%d %.7f\n", i+1, path_cost[i]);
    }
    if (verbose>=1)
        printf("The number of paths: %ld, total cost is %.7f, final path cost is: %.7f. #path_set %ld\n",
               path_cost.size(), cost_sum, path_cost[path_cost.size() - 1], path_set.size());

    return make_output2(org_graph.get(), path_set, cost_sum);
}

BOOST_PYTHON_MODULE(wrapper) {
    Py_Initialize();

    p::def("solve", &solve);

};


// Copyright (C) 2011 by the BEM++ Authors
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to deal
// in the Software without restriction, including without limitation the rights
// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
// copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
// THE SOFTWARE.

#include <armadillo>
#include <iostream>
#include <memory> // auto_ptr

#include "grid/entity.hpp"
#include "grid/entity_iterator.hpp"
#include "grid/geometry.hpp"
#include "grid/grid.hpp"
#include "grid/grid_factory.hpp"
#include "grid/grid_view.hpp"
#include "grid/index_set.hpp"
#include "grid/vtk_writer.hpp"

using namespace Bempp;
using std::cout;
using std::endl;

/**
  Load a mesh from a Gmsh file and export a VTK file 'vtkhead.vtu' containing a
  data series 'z_coord' which maps each vertex to its z coordinate. This series
  can be subsequently visulatised in ParaView.
  */
int main()
{
    const char MESH_FNAME[] = "head.gmsh";

    // Import the grid
    GridParameters params;
    params.topology = GridParameters::TRIANGULAR;

    std::auto_ptr<Grid> grid(GridFactory::importGmshGrid(params, std::string(MESH_FNAME),
                             true, // verbose
                             false)); // insertBoundarySegments

    // Create a leaf view
    std::auto_ptr<GridView> leafGridView(grid->leafView());
    const IndexSet& indexSet = leafGridView->indexSet();

    const int nVertices = leafGridView->entityCount(2);
    arma::Mat<double> data(1,nVertices);
    arma::Col<double> elementCenter;

    cout << "Traversing the grid..." << endl;
    std::auto_ptr<EntityIterator<2> > leafVertexIt(leafGridView->entityIterator<2>());
    while (!leafVertexIt->finished()) {
        const Entity<2>& e = leafVertexIt->entity();

        const Geometry& geo = e.geometry();
        geo.center(elementCenter);
        data(0,indexSet.entityIndex(e)) = elementCenter(2); // the z coordinate

        leafVertexIt->next();
    }

    cout << "Gathering data..." << endl;
    std::auto_ptr<VtkWriter> vtkWriter = leafGridView->vtkWriter();
    vtkWriter->addVertexData(data, "z_coord");
    cout << "Exporting data..." << endl;
    std::string outputFname = vtkWriter->write("vtkhead");
    cout << "Data exported to '" << outputFname << "'." << endl;
}


//  Copyright John Maddock 2006.
//  Copyright Paul A. Bristow 2007
//  Use, modification and distribution are subject to the
//  Boost Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)

#include <pch.hpp>

#include <cmath>
#include <math.h>
#include <boost/limits.hpp>
#include <boost/math/concepts/real_concept.hpp>
#include <boost/math/special_functions/fpclassify.hpp>
#define BOOST_TEST_MAIN
#include <boost/test/unit_test.hpp>
#include <iostream>
#include <iomanip>

#ifdef _MSC_VER
#pragma warning(disable: 4127 4146) //  conditional expression is constant
#endif

const char* method_name(const boost::math::detail::native_tag&)
{
   return "Native";
}

const char* method_name(const boost::math::detail::generic_tag<true>&)
{
   return "Generic (with numeric limits)";
}

const char* method_name(const boost::math::detail::generic_tag<false>&)
{
   return "Generic (without numeric limits)";
}

const char* method_name(const boost::math::detail::ieee_tag&)
{
   return "IEEE std";
}

const char* method_name(const boost::math::detail::ieee_copy_all_bits_tag&)
{
   return "IEEE std, copy all bits";
}

const char* method_name(const boost::math::detail::ieee_copy_leading_bits_tag&)
{
   return "IEEE std, copy leading bits";
}

template <class T>
void test_classify(T t, const char* type)
{
   std::cout << "Testing type " << type << std::endl;

   typedef typename boost::math::detail::fp_traits<T>::type traits;
   typedef typename traits::method method;

   std::cout << "Evaluation method = " << method_name(method()) << std::endl;

   t = 2;
   T u = 2;
   BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_NORMAL);
   BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_NORMAL);
   BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
   BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
   BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), true);
   BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), true);
   if(std::numeric_limits<T>::is_specialized)
   {
      t = (std::numeric_limits<T>::max)();
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_NORMAL);
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_NORMAL);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), true);
      t = (std::numeric_limits<T>::min)();
      if(t != 0)
      {
         BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_NORMAL);
         BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_NORMAL);
         BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
         BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
         BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), true);
         BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), true);
      }
   }
   if(std::numeric_limits<T>::has_denorm)
   {
      t = (std::numeric_limits<T>::min)();
      t /= 2;
      if(t != 0)
      {
         BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_SUBNORMAL);
         BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_SUBNORMAL);
         BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
         BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
         BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
      }
      t = std::numeric_limits<T>::denorm_min();
      if((t != 0) && (t < (std::numeric_limits<T>::min)()))
      {
         BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_SUBNORMAL);
         BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_SUBNORMAL);
         BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
         BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
         BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
         BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
      }
   }
   else
   {
      std::cout << "Denormalised forms not tested" << std::endl;
   }
   t = 0;
   BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_ZERO);
   BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_ZERO);
   BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
   BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
   BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
   t /= -u; // create minus zero if it exists
   BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_ZERO);
   BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_ZERO);
   BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), true);
   BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), true);
   BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
   BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
   // infinity:
   if(std::numeric_limits<T>::has_infinity)
   {
      // At least one std::numeric_limits<T>::infinity)() returns zero
      // (Compaq true64 cxx), hence the check.
      t = (std::numeric_limits<T>::infinity)();
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_INFINITE);
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_INFINITE);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
#if !defined(__BORLANDC__) && !(defined(__DECCXX) && !defined(_IEEE_FP))
      // divide by zero on Borland triggers a C++ exception :-(
      // divide by zero on Compaq CXX triggers a C style signal :-(
      t = 2;
      u = 0;
      t /= u;
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_INFINITE);
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_INFINITE);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
      t = -2;
      t /= u;
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_INFINITE);
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_INFINITE);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
#else
      std::cout << "Infinities from divide by zero not tested" << std::endl;
#endif
   }
   else
   {
      std::cout << "Infinity not tested" << std::endl;
   }
#ifndef __BORLANDC__
   // NaN's:
   // Note that Borland throws an exception if we even try to obtain a Nan
   // by calling std::numeric_limits<T>::quiet_NaN() !!!!!!!
   if(std::numeric_limits<T>::has_quiet_NaN)
   {
      t = std::numeric_limits<T>::quiet_NaN();
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_NAN);
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_NAN);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
   }
   else
   {
      std::cout << "Quiet NaN's not tested" << std::endl;
   }
   if(std::numeric_limits<T>::has_signaling_NaN)
   {
      t = std::numeric_limits<T>::signaling_NaN();
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(t), (int)FP_NAN);
      BOOST_CHECK_EQUAL((::boost::math::fpclassify)(-t), (int)FP_NAN);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isfinite)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isinf)(-t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnan)(-t), true);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(t), false);
      BOOST_CHECK_EQUAL((::boost::math::isnormal)(-t), false);
   }
   else
   {
      std::cout << "Signaling NaN's not tested" << std::endl;
   }
#endif
}

BOOST_AUTO_TEST_CASE( test_main )
{
   BOOST_MATH_CONTROL_FP;
   // start by printing some information:
#ifdef isnan
   std::cout << "Platform has isnan macro." << std::endl;
#endif
#ifdef fpclassify
   std::cout << "Platform has fpclassify macro." << std::endl;
#endif
#ifdef BOOST_HAS_FPCLASSIFY
   std::cout << "Platform has FP_NORMAL macro." << std::endl;
#endif
   std::cout << "FP_ZERO: " << (int)FP_ZERO << std::endl;
   std::cout << "FP_NORMAL: " << (int)FP_NORMAL << std::endl;
   std::cout << "FP_INFINITE: " << (int)FP_INFINITE << std::endl;
   std::cout << "FP_NAN: " << (int)FP_NAN << std::endl;
   std::cout << "FP_SUBNORMAL: " << (int)FP_SUBNORMAL << std::endl;

   // then run the tests:
   test_classify(float(0), "float");
   test_classify(double(0), "double");
   // long double support for fpclassify is considered "core" so we always test it
   // even when long double support is turned off via BOOST_MATH_NO_LONG_DOUBLE_MATH_FUNCTIONS
   test_classify((long double)(0), "long double");
   test_classify((boost::math::concepts::real_concept)(0), "real_concept");

   // We should test with integer types as well:
   test_classify(int(0), "int");
   test_classify(unsigned(0), "unsigned");
}

/*
Autorun "i:\Boost-sandbox\math_toolkit\libs\math\test\MSVC80\debug\test_classify.exe"
Running 1 test case...
FP_ZERO: 0
FP_NORMAL: 1
FP_INFINITE: 2
FP_NAN: 3
FP_SUBNORMAL: 4
Testing type float
Testing type double
Testing type long double
Testing type real_concept
Denormalised forms not tested
Infinity not tested
Quiet NaN's not tested
Signaling NaN's not tested
Test suite "Test Program" passed with:
  79 assertions out of 79 passed
  1 test case out of 1 passed
  Test case "test_main_caller( argc, argv )" passed with:
    79 assertions out of 79 passed

*/


// Boost.Geometry (aka GGL, Generic Geometry Library)
// Unit Test

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

// Contributed and/or modified by Menelaos Karavelas, on behalf of Oracle

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

#ifndef BOOST_GEOMETRY_TEST_ENVELOPE_EXPAND_ON_SPHEROID_HPP
#define BOOST_GEOMETRY_TEST_ENVELOPE_EXPAND_ON_SPHEROID_HPP

#include <cmath>
#include <cstddef>
#include <algorithm>

#include <boost/type_traits/is_same.hpp>

#include <boost/geometry/core/access.hpp>
#include <boost/geometry/core/coordinate_dimension.hpp>
#include <boost/geometry/core/cs.hpp>

#include <boost/geometry/util/condition.hpp>
#include <boost/geometry/util/math.hpp>

#include <boost/geometry/views/detail/indexed_point_view.hpp>

#include <boost/geometry/algorithms/assign.hpp>

template <typename Units>
char const* units2string()
{
    if (BOOST_GEOMETRY_CONDITION((boost::is_same<Units, bg::degree>::value)))
    {
        return "degrees";
    }
    return "radians";
}

template <typename CoordinateSystem>
struct other_system_info
{};

template <>
struct other_system_info<bg::cs::spherical_equatorial<bg::radian> >
{
    typedef bg::degree units;
    typedef bg::cs::spherical_equatorial<units> type;

    template <typename T>
    static inline T convert(T const& value)
    {
        return value * bg::math::r2d<T>();
    }
};

template <>
struct other_system_info<bg::cs::spherical_equatorial<bg::degree> >
{
    typedef bg::radian units;
    typedef bg::cs::spherical_equatorial<units> type;

    template <typename T>
    static inline T convert(T const& value)
    {
        return value * bg::math::d2r<T>();
    }
};

template <>
struct other_system_info<bg::cs::geographic<bg::radian> >
{
    typedef bg::degree units;
    typedef bg::cs::geographic<units> type;

    template <typename T>
    static inline T convert(T const& value)
    {
        return value * bg::math::r2d<T>();
    }
};

template <>
struct other_system_info<bg::cs::geographic<bg::degree> >
{
    typedef bg::radian units;
    typedef bg::cs::geographic<units> type;

    template <typename T>
    static inline T convert(T const& value)
    {
        return value * bg::math::d2r<T>();
    }
};

class equals_with_tolerance
{
private:
    double m_tolerence;

    template <typename T>
    static inline T const& get_max(T const& a, T const& b, T const& c)
    {
        return (std::max)((std::max)(a, b), c);
    }

    template <typename T>
    static inline bool check_close(T const& a, T const& b, double tol)
    {
        return (a == b)
            || (std::abs(a - b) <= tol * get_max(std::abs(a), std::abs(b), 1.0));
    }

public:
    equals_with_tolerance(double tolerence) : m_tolerence(tolerence) {}

    template <typename T>
    inline bool operator()(T const& value1, T const& value2) const
    {
        return check_close(value1, value2, m_tolerence);
    }
};

template
<
    typename Box1,
    typename Box2 = Box1,
    std::size_t DimensionCount = bg::dimension<Box1>::value
>
struct box_equals
{
    static inline bool apply(Box1 const& box1, Box2 const& box2, double tol)
    {
        equals_with_tolerance equals(tol);

#ifndef BOOST_GEOMETRY_TEST_ENABLE_FAILING
        // check latitude with tolerance when necessary
        return bg::math::equals(bg::get<0, 0>(box1), bg::get<0, 0>(box2))
            && (bg::get<0, 1>(box1) < 0
                ? equals(bg::get<0, 1>(box1), bg::get<0, 1>(box2))
                : bg::math::equals(bg::get<0, 1>(box1), bg::get<0, 1>(box2)))
            && bg::math::equals(bg::get<1, 0>(box1), bg::get<1, 0>(box2))
            && (bg::get<1, 1>(box1) > 0
                ? equals(bg::get<1, 1>(box1), bg::get<1, 1>(box2))
                : bg::math::equals(bg::get<1, 1>(box1), bg::get<1, 1>(box2)));
#else
        // check latitude with tolerance when necessary
        return bg::get<0, 0>(box1) == bg::get<0, 0>(box2)
            && (bg::get<0, 1>(box1) < 0
                ? equals(bg::get<0, 1>(box1), bg::get<0, 1>(box2))
                : bg::get<0, 1>(box1) == bg::get<0, 1>(box2))
            && bg::get<1, 0>(box1) == bg::get<1, 0>(box2)
            && (bg::get<1, 1>(box1) > 0
                ? equals(bg::get<1, 1>(box1), bg::get<1, 1>(box2))
                : bg::get<1, 1>(box1) == bg::get<1, 1>(box2));
#endif
    }
};

template <typename Box1, typename Box2>
struct box_equals<Box1, Box2, 3>
{
    static inline bool apply(Box1 const& box1, Box2 const& box2, double tol)
    {
#ifndef BOOST_GEOMETRY_TEST_ENABLE_FAILING
        equals_with_tolerance equals(tol);

        return box_equals<Box1, Box2, 2>::apply(box1, box2, tol)
            && equals(bg::get<0, 2>(box1), bg::get<0, 2>(box2))
            && equals(bg::get<1, 2>(box1), bg::get<1, 2>(box2));
#else
        return box_equals<Box1, Box2, 2>::apply(box1, box2, tol)
            && bg::get<0, 2>(box1) == bg::get<0, 2>(box2)
            && bg::get<1, 2>(box1) == bg::get<1, 2>(box2);
#endif
    }
};

template <typename Box, std::size_t Dimension = bg::dimension<Box>::value>
struct initialize_box
{
    static inline void apply(Box& box,
                             double lon_min, double lat_min, double,
                             double lon_max, double lat_max, double)
    {
        bg::detail::indexed_point_view<Box, bg::min_corner> p_min(box);
        bg::detail::indexed_point_view<Box, bg::max_corner> p_max(box);

        bg::assign_values(p_min, lon_min, lat_min);
        bg::assign_values(p_max, lon_max, lat_max);
    }
};

template <typename Box>
struct initialize_box<Box, 3>
{
    static inline void apply(Box& box,
                             double lon_min, double lat_min, double height_min,
                             double lon_max, double lat_max, double height_max)
    {
        bg::detail::indexed_point_view<Box, bg::min_corner> p_min(box);
        bg::detail::indexed_point_view<Box, bg::max_corner> p_max(box);

        bg::assign_values(p_min, lon_min, lat_min, height_min);
        bg::assign_values(p_max, lon_max, lat_max, height_max);
    }
};

#endif // BOOST_GEOMETRY_TEST_ENVELOPE_EXPAND_ON_SPHEROID_HPP


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// 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)
//
/// \file parelm_test.cpp
/// \brief Add your file description here.

//#define MCRL2_LPS_PARELM_DEBUG

#include "mcrl2/data/consistency.h"
#include "mcrl2/data/detail/test_rewriters.h"
#include "mcrl2/lps/multi_action.h"
#include "mcrl2/lps/print.h"
#include <boost/test/minimal.hpp>
#include <iostream>
#include <string>

using namespace mcrl2;
using namespace mcrl2::data;
using namespace mcrl2::lps;

/// \brief Returns a data variable of type Nat with a given name
/// \param name A string
/// \return A data variable of type Nat with a given name
data::variable nat(const std::string& name)
{
  return data::variable(core::identifier_string(name), data::sort_nat::nat());
}

process::action act(const std::string& name, data_expression_list parameters)
{
  std::vector<sort_expression> sorts;
  for (const auto & parameter : parameters)
  {
    sorts.push_back(parameter.sort());
  }
  process::action_label label(name, sort_expression_list(sorts.begin(), sorts.end()));
  return process::action(label, parameters);
}

void test_multi_actions(const process::action_list& a, const process::action_list& b, const data_expression& expected_result = data::undefined_data_expression())
{
  std::cout << "--- test_multi_actions ---" << std::endl;
  data_expression result = equal_multi_actions(multi_action(a), multi_action(b));
  std::cout << "a               = " << lps::pp(a) << std::endl;
  std::cout << "b               = " << lps::pp(b) << std::endl;
  std::cout << "result          = " << lps::pp(result) << std::endl;
  std::cout << "expected_result = " << lps::pp(expected_result) << std::endl;
  BOOST_CHECK(expected_result == data::undefined_data_expression() || result == expected_result);
}

void test_equal_multi_actions()
{
  namespace d = data;

  data_expression d1 = nat("d1");
  data_expression d2 = nat("d2");
  data_expression d3 = nat("d3");
  data_expression d4 = nat("d4");
  process::action_list a1  ( { act("a", data_expression_list({ d1 })) });
  process::action_list a2  ( { act("a", data_expression_list({ d2 })) });
  process::action_list b1  ( { act("b", data_expression_list({ d1 })) });
  process::action_list b2  ( { act("b", data_expression_list({ d2 })) });
  process::action_list a11 ( { act("a", data_expression_list({ d1 })), act("a", data_expression_list({ d1 })) });
  process::action_list a12 ( { act("a", data_expression_list({ d1 })), act("a", data_expression_list({ d2 })) });
  process::action_list a21 ( { act("a", data_expression_list({ d2 })), act("a", data_expression_list({ d1 })) });
  process::action_list a22 ( { act("a", data_expression_list({ d2 })), act("a", data_expression_list({ d2 })) });
  process::action_list a34 ( { act("a", data_expression_list({ d3 })), act("a", data_expression_list({ d4 })) });
  process::action_list a12b1 ( { act("a", data_expression_list({ d1 })), act("a", data_expression_list({ d2 })), act("b", data_expression_list({ d1 })) });
  process::action_list a34b2 ( { act("a", data_expression_list({ d3 })), act("a", data_expression_list({ d4 })), act("b", data_expression_list({ d2 })) });
  test_multi_actions(a1,  a1, d::true_());
  test_multi_actions(a1,  a2, d::equal_to(d1, d2));
  test_multi_actions(a11, a11, d::true_());
  test_multi_actions(a12, a21, d::true_());
  test_multi_actions(a21, a12, d::true_());
  test_multi_actions(a11, a22, d::equal_to(d1, d2));
  test_multi_actions(a1, a12,  d::false_());
  test_multi_actions(a1, b1,   d::false_());
  test_multi_actions(a12, a34);
  test_multi_actions(a12b1, a34b2);

  data_expression m1 = equal_multi_actions(multi_action(a12), multi_action(a34));
  data_expression m2 = equal_multi_actions(multi_action(a34), multi_action(a12));
  data_expression n1 = data::detail::normalize_equality(m1);
  data_expression n2 = data::detail::normalize_equality(m2);
  data_expression p1 = data::detail::normalize_and_or(n1);
  data_expression p2 = data::detail::normalize_and_or(n2);
  std::cout << "m1 = " << data::pp(m1) << std::endl;
  std::cout << "m2 = " << data::pp(m2) << std::endl;
  std::cout << "n1 = " << data::pp(n1) << std::endl;
  std::cout << "n2 = " << data::pp(n2) << std::endl;
  std::cout << "p1 = " << data::pp(p1) << std::endl;
  std::cout << "p2 = " << data::pp(p2) << std::endl;
  BOOST_CHECK(p1 == p2);
}

void test_pp()
{
  data_expression d1 = nat("d1");
  data_expression d2 = nat("d2");
  process::action_list a1  ( { act("a", data_expression_list({ d1 })) });
  process::action_list a2  ( { act("a", data_expression_list({ d2 })) });
  process::action_list b1  ( { act("b", data_expression_list({ d1 })) });
  process::action_list a11 ( { act("a", data_expression_list({ d1 })), act("a", data_expression_list({ d1 })) });
  multi_action m(a11);
  std::string s = lps::pp(m);
  std::cout << "s = " << s << std::endl;
  BOOST_CHECK(s == "a(d1)|a(d1)");
}

int test_main(int argc, char* argv[])
{
  test_equal_multi_actions();
  test_pp();

  return 0;
}


//----------------------------------------------------------------------------
/** @file SgFastLogTest.cpp
    Unit tests for SgFastLog.
*/
//----------------------------------------------------------------------------

#include "SgSystem.h"

#include <cmath>
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include "SgFastLog.h"

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

namespace {

BOOST_AUTO_TEST_CASE(SgFastLogTest_Basic)
{
    SgFastLog fastLog(10);
    const float epsilon = 0.1;
    BOOST_CHECK_CLOSE(fastLog.Log(1), 0.0f, epsilon);
    BOOST_CHECK_CLOSE(fastLog.Log(M_E), 1.0f, epsilon);
    BOOST_CHECK_CLOSE(fastLog.Log(pow(M_E, 2.0)), 2.0f, epsilon);
}

} // namespace

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


// Copyright John Maddock 2010.
// Copyright Paul A. Bristow 2010.

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

#ifndef BOOST_MATH_DISTRIBUTIONS_INVERSE_CHI_SQUARED_HPP
#define BOOST_MATH_DISTRIBUTIONS_INVERSE_CHI_SQUARED_HPP

#include <boost/math/distributions/fwd.hpp>
#include <boost/math/special_functions/gamma.hpp> // for incomplete beta.
#include <boost/math/distributions/complement.hpp> // for complements.
#include <boost/math/distributions/detail/common_error_handling.hpp> // for error checks.
#include <boost/math/special_functions/fpclassify.hpp> // for isfinite

// See http://en.wikipedia.org/wiki/Scaled-inverse-chi-square_distribution
// for definitions of this scaled version.
// See http://en.wikipedia.org/wiki/Inverse-chi-square_distribution
// for unscaled version.

// http://reference.wolfram.com/mathematica/ref/InverseChiSquareDistribution.html
// Weisstein, Eric W. "Inverse Chi-Squared Distribution." From MathWorld--A Wolfram Web Resource.
// http://mathworld.wolfram.com/InverseChi-SquaredDistribution.html

#include <utility>

namespace boost{ namespace math{

namespace detail
{
  template <class RealType, class Policy>
  inline bool check_inverse_chi_squared( // Check both distribution parameters.
        const char* function,
        RealType degrees_of_freedom, // degrees_of_freedom (aka nu).
        RealType scale,  // scale (aka sigma^2)
        RealType* result,
        const Policy& pol)
  {
     return check_scale(function, scale, result, pol)
       && check_df(function, degrees_of_freedom,
       result, pol);
  } // bool check_inverse_chi_squared
} // namespace detail

template <class RealType = double, class Policy = policies::policy<> >
class inverse_chi_squared_distribution
{
public:
   typedef RealType value_type;
   typedef Policy policy_type;

   inverse_chi_squared_distribution(RealType df, RealType l_scale) : m_df(df), m_scale (l_scale)
   {
      RealType result;
      detail::check_df(
         "boost::math::inverse_chi_squared_distribution<%1%>::inverse_chi_squared_distribution",
         m_df, &result, Policy())
         && detail::check_scale(
"boost::math::inverse_chi_squared_distribution<%1%>::inverse_chi_squared_distribution",
         m_scale, &result,  Policy());
   } // inverse_chi_squared_distribution constructor

   inverse_chi_squared_distribution(RealType df = 1) : m_df(df)
   {
      RealType result;
      m_scale = 1 / m_df ; // Default scale = 1 / degrees of freedom (Wikipedia definition 1).
      detail::check_df(
         "boost::math::inverse_chi_squared_distribution<%1%>::inverse_chi_squared_distribution",
         m_df, &result, Policy());
   } // inverse_chi_squared_distribution

   RealType degrees_of_freedom()const
   {
      return m_df; // aka nu
   }
   RealType scale()const
   {
      return m_scale;  // aka xi
   }

   // Parameter estimation:  NOT implemented yet.
   //static RealType find_degrees_of_freedom(
   //   RealType difference_from_variance,
   //   RealType alpha,
   //   RealType beta,
   //   RealType variance,
   //   RealType hint = 100);

private:
   // Data members:
   RealType m_df;  // degrees of freedom are treated as a real number.
   RealType m_scale;  // distribution scale.

}; // class chi_squared_distribution

typedef inverse_chi_squared_distribution<double> inverse_chi_squared;

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> range(const inverse_chi_squared_distribution<RealType, Policy>& /*dist*/)
{  // Range of permissible values for random variable x.
   using boost::math::tools::max_value;
   return std::pair<RealType, RealType>(static_cast<RealType>(0), max_value<RealType>()); // 0 to + infinity.
}

template <class RealType, class Policy>
inline const std::pair<RealType, RealType> support(const inverse_chi_squared_distribution<RealType, Policy>& /*dist*/)
{  // Range of supported values for random variable x.
   // This is range where cdf rises from 0 to 1, and outside it, the pdf is zero.
   return std::pair<RealType, RealType>(static_cast<RealType>(0), tools::max_value<RealType>()); // 0 to + infinity.
}

template <class RealType, class Policy>
RealType pdf(const inverse_chi_squared_distribution<RealType, Policy>& dist, const RealType& x)
{
   BOOST_MATH_STD_USING  // for ADL of std functions.
   RealType df = dist.degrees_of_freedom();
   RealType scale = dist.scale();
   RealType error_result;

   static const char* function = "boost::math::pdf(const inverse_chi_squared_distribution<%1%>&, %1%)";

   if(false == detail::check_inverse_chi_squared
     (function, df, scale, &error_result, Policy())
     )
   { // Bad distribution.
      return error_result;
   }
   if((x < 0) || !(boost::math::isfinite)(x))
   { // Bad x.
      return policies::raise_domain_error<RealType>(
         function, "inverse Chi Square parameter was %1%, but must be >= 0 !", x, Policy());
   }

   if(x == 0)
   { // Treat as special case.
     return 0;
   }
   // Wikipedia scaled inverse chi sq (df, scale) related to inv gamma (df/2, df * scale /2)
   // so use inverse gamma pdf with shape = df/2, scale df * scale /2
   // RealType shape = df /2; // inv_gamma shape
   // RealType scale = df * scale/2; // inv_gamma scale
   // RealType result = gamma_p_derivative(shape, scale / x, Policy()) * scale / (x * x);
   RealType result = df * scale/2 / x;
   if(result < tools::min_value<RealType>())
      return 0; // Random variable is near enough infinite.
   result = gamma_p_derivative(df/2, result, Policy()) * df * scale/2;
   if(result != 0) // prevent 0 / 0,  gamma_p_derivative -> 0 faster than x^2
      result /= (x * x);
   return result;
} // pdf

template <class RealType, class Policy>
inline RealType cdf(const inverse_chi_squared_distribution<RealType, Policy>& dist, const RealType& x)
{
   static const char* function = "boost::math::cdf(const inverse_chi_squared_distribution<%1%>&, %1%)";
   RealType df = dist.degrees_of_freedom();
   RealType scale = dist.scale();
   RealType error_result;

   if(false ==
       detail::check_inverse_chi_squared(function, df, scale, &error_result, Policy())
     )
   { // Bad distribution.
      return error_result;
   }
   if((x < 0) || !(boost::math::isfinite)(x))
   { // Bad x.
      return policies::raise_domain_error<RealType>(
         function, "inverse Chi Square parameter was %1%, but must be >= 0 !", x, Policy());
   }
   if (x == 0)
   { // Treat zero as a special case.
     return 0;
   }
   // RealType shape = df /2; // inv_gamma shape,
   // RealType scale = df * scale/2; // inv_gamma scale,
   // result = boost::math::gamma_q(shape, scale / x, Policy()); // inverse_gamma code.
   return boost::math::gamma_q(df / 2, (df * (scale / 2)) / x, Policy());
} // cdf

template <class RealType, class Policy>
inline RealType quantile(const inverse_chi_squared_distribution<RealType, Policy>& dist, const RealType& p)
{
   using boost::math::gamma_q_inv;
   RealType df = dist.degrees_of_freedom();
   RealType scale = dist.scale();

   static const char* function = "boost::math::quantile(const inverse_chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, df, &error_result, Policy())
         && detail::check_probability(
            function, p, &error_result, Policy()))
   {
      return error_result;
   }
   if(false == detail::check_probability(
            function, p, &error_result, Policy()))
   {
      return error_result;
   }
   // RealType shape = df /2; // inv_gamma shape,
   // RealType scale = df * scale/2; // inv_gamma scale,
   // result = scale / gamma_q_inv(shape, p, Policy());
      RealType result = gamma_q_inv(df /2, p, Policy());
      if(result == 0)
         return policies::raise_overflow_error<RealType, Policy>(function, "Random variable is infinite.", Policy());
      result = df * (scale / 2) / result;
      return result;
} // quantile

template <class RealType, class Policy>
inline RealType cdf(const complemented2_type<inverse_chi_squared_distribution<RealType, Policy>, RealType>& c)
{
   using boost::math::gamma_q_inv;
   RealType const& df = c.dist.degrees_of_freedom();
   RealType const& scale = c.dist.scale();
   RealType const& x = c.param;
   static const char* function = "boost::math::cdf(const inverse_chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(
         function, df, &error_result, Policy()))
   {
      return error_result;
   }
   if (x == 0)
   { // Treat zero as a special case.
     return 1;
   }
   if((x < 0) || !(boost::math::isfinite)(x))
   {
      return policies::raise_domain_error<RealType>(
         function, "inverse Chi Square parameter was %1%, but must be > 0 !", x, Policy());
   }
   // RealType shape = df /2; // inv_gamma shape,
   // RealType scale = df * scale/2; // inv_gamma scale,
   // result = gamma_p(shape, scale/c.param, Policy()); use inv_gamma.

   return gamma_p(df / 2, (df * scale/2) / x, Policy()); // OK
} // cdf(complemented

template <class RealType, class Policy>
inline RealType quantile(const complemented2_type<inverse_chi_squared_distribution<RealType, Policy>, RealType>& c)
{
   using boost::math::gamma_q_inv;

   RealType const& df = c.dist.degrees_of_freedom();
   RealType const& scale = c.dist.scale();
   RealType const& q = c.param;
   static const char* function = "boost::math::quantile(const inverse_chi_squared_distribution<%1%>&, %1%)";
   // Error check:
   RealType error_result;
   if(false == detail::check_df(function, df, &error_result, Policy()))
   {
      return error_result;
   }
   if(false == detail::check_probability(function, q, &error_result, Policy()))
   {
      return error_result;
   }
   // RealType shape = df /2; // inv_gamma shape,
   // RealType scale = df * scale/2; // inv_gamma scale,
   // result = scale / gamma_p_inv(shape, q, Policy());  // using inv_gamma.
   RealType result = gamma_p_inv(df/2, q, Policy());
   if(result == 0)
      return policies::raise_overflow_error<RealType, Policy>(function, "Random variable is infinite.", Policy());
   result = (df * scale / 2) / result;
   return result;
} // quantile(const complement

template <class RealType, class Policy>
inline RealType mean(const inverse_chi_squared_distribution<RealType, Policy>& dist)
{ // Mean of inverse Chi-Squared distribution.
   RealType df = dist.degrees_of_freedom();
   RealType scale = dist.scale();

   static const char* function = "boost::math::mean(const inverse_chi_squared_distribution<%1%>&)";
   if(df <= 2)
      return policies::raise_domain_error<RealType>(
         function,
         "inverse Chi-Squared distribution only has a mode for degrees of freedom > 2, but got degrees of freedom = %1%.",
         df, Policy());
  return (df * scale) / (df - 2);
} // mean

template <class RealType, class Policy>
inline RealType variance(const inverse_chi_squared_distribution<RealType, Policy>& dist)
{ // Variance of inverse Chi-Squared distribution.
   RealType df = dist.degrees_of_freedom();
   RealType scale = dist.scale();
   static const char* function = "boost::math::variance(const inverse_chi_squared_distribution<%1%>&)";
   if(df <= 4)
   {
      return policies::raise_domain_error<RealType>(
         function,
         "inverse Chi-Squared distribution only has a variance for degrees of freedom > 4, but got degrees of freedom = %1%.",
         df, Policy());
   }
   return 2 * df * df * scale * scale / ((df - 2)*(df - 2) * (df - 4));
} // variance

template <class RealType, class Policy>
inline RealType mode(const inverse_chi_squared_distribution<RealType, Policy>& dist)
{ // mode is not defined in Mathematica.
  // See Discussion section http://en.wikipedia.org/wiki/Talk:Scaled-inverse-chi-square_distribution
  // for origin of the formula used below.

   RealType df = dist.degrees_of_freedom();
   RealType scale = dist.scale();
   static const char* function = "boost::math::mode(const inverse_chi_squared_distribution<%1%>&)";
   if(df < 0)
      return policies::raise_domain_error<RealType>(
         function,
         "inverse Chi-Squared distribution only has a mode for degrees of freedom >= 0, but got degrees of freedom = %1%.",
         df, Policy());
   return (df * scale) / (df + 2);
}

//template <class RealType, class Policy>
//inline RealType median(const inverse_chi_squared_distribution<RealType, Policy>& dist)
//{ // Median is given by Quantile[dist, 1/2]
//   RealType df = dist.degrees_of_freedom();
//   if(df <= 1)
//      return tools::domain_error<RealType>(
//         BOOST_CURRENT_FUNCTION,
//         "The inverse_Chi-Squared distribution only has a median for degrees of freedom >= 0, but got degrees of freedom = %1%.",
//         df);
//   return df;
//}
// Now implemented via quantile(half) in derived accessors.

template <class RealType, class Policy>
inline RealType skewness(const inverse_chi_squared_distribution<RealType, Policy>& dist)
{
   BOOST_MATH_STD_USING // For ADL
   RealType df = dist.degrees_of_freedom();
   static const char* function = "boost::math::skewness(const inverse_chi_squared_distribution<%1%>&)";
   if(df <= 6)
      return policies::raise_domain_error<RealType>(
         function,
         "inverse Chi-Squared distribution only has a skewness for degrees of freedom > 6, but got degrees of freedom = %1%.",
         df, Policy());

   return 4 * sqrt (2 * (df - 4)) / (df - 6);  // Not a function of scale.
}

template <class RealType, class Policy>
inline RealType kurtosis(const inverse_chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   static const char* function = "boost::math::kurtosis(const inverse_chi_squared_distribution<%1%>&)";
   if(df <= 8)
      return policies::raise_domain_error<RealType>(
         function,
         "inverse Chi-Squared distribution only has a kurtosis for degrees of freedom > 8, but got degrees of freedom = %1%.",
         df, Policy());

   return kurtosis_excess(dist) + 3;
}

template <class RealType, class Policy>
inline RealType kurtosis_excess(const inverse_chi_squared_distribution<RealType, Policy>& dist)
{
   RealType df = dist.degrees_of_freedom();
   static const char* function = "boost::math::kurtosis(const inverse_chi_squared_distribution<%1%>&)";
   if(df <= 8)
      return policies::raise_domain_error<RealType>(
         function,
         "inverse Chi-Squared distribution only has a kurtosis excess for degrees of freedom > 8, but got degrees of freedom = %1%.",
         df, Policy());

   return 12 * (5 * df - 22) / ((df - 6 )*(df - 8));  // Not a function of scale.
}

//
// Parameter estimation comes last:
//

} // namespace math
} // namespace boost

// This include must be at the end, *after* the accessors
// for this distribution have been defined, in order to
// keep compilers that support two-phase lookup happy.
#include <boost/math/distributions/detail/derived_accessors.hpp>

#endif // BOOST_MATH_DISTRIBUTIONS_INVERSE_CHI_SQUARED_HPP


// Bring in gtest
#include <gtest/gtest.h>
#include <boost/cstdint.hpp>

// Helpful functions from schweizer_messer
#include <sparse_block_matrix/sparse_block_matrix.h>
#include <boost/random/linear_congruential.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <sm/eigen/gtest.hpp>
#include "sbm_gtest.hpp"

typedef boost::minstd_rand base_generator_type;
// Returns a float r in the range 0.0 < f < 1.0.
inline double rand1() {
    static base_generator_type generator;

    // Define a uniform random number distribution which produces "double"
    // values between 0 and 1 (0 inclusive, 1 exclusive).
    static boost::uniform_real<> uni_dist(0, 1);
    static boost::variate_generator<base_generator_type&, boost::uniform_real<> > uni(generator, uni_dist);

    return uni();
}

template <typename M>
sparse_block_matrix::SparseBlockMatrix<M> buildRandomMatrix(Eigen::MatrixXi rows, Eigen::MatrixXi cols,
                                                            double probabilityOfABlockBeingFilled) {
    typedef M matrix_t;

    sparse_block_matrix::SparseBlockMatrix<matrix_t> Mx(rows, cols);
    EXPECT_EQ((size_t)rows.size(), Mx.rowBlockIndices().size());
    EXPECT_EQ((size_t)cols.size(), Mx.colBlockIndices().size());

    size_t nonzeroBlocks = 0;
    size_t nonzeroElements = 0;
    for (int cb = 0; cb < Mx.bCols(); cb++) {
        for (int rb = 0; rb < Mx.bRows(); rb++) {
            if (rand1() < probabilityOfABlockBeingFilled) {
                // check all versions of the "block" function.
                matrix_t* B = Mx.block(rb, cb);
                EXPECT_EQ((matrix_t*)NULL, B) << "Block at " << rb << "," << cb << " should be null";

                B = Mx.block(rb, cb, false);
                EXPECT_EQ((matrix_t*)NULL, B) << "Block at " << rb << "," << cb << " should be null";

                B = Mx.block(rb, cb, true);
                EXPECT_NE((matrix_t*)NULL, B);
                EXPECT_EQ(B->rows(), Mx.rowsOfBlock(rb));
                EXPECT_EQ(B->cols(), Mx.colsOfBlock(cb));
                B->setRandom();
                ++nonzeroBlocks;
                nonzeroElements += B->rows() * B->cols();
                EXPECT_EQ(nonzeroBlocks, Mx.nonZeroBlocks());
                EXPECT_EQ(nonzeroElements, Mx.nonZeros());

            } else {
                // Check both versions of this function
                matrix_t* B = Mx.block(rb, cb);
                EXPECT_EQ((matrix_t*)NULL, B) << "Block at " << rb << "," << cb << " should be null";

                B = Mx.block(rb, cb, false);
                EXPECT_EQ((matrix_t*)NULL, B) << "Block at " << rb << "," << cb << " should be null";
            }
        }
    }

    return Mx;
}

TEST(sparse_block_matrixTestSuite, testBlock) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(3);
    rows << 3, 6, 11;
    VectorXi cols(4);
    cols << 4, 9, 12, 18;

    SparseBlockMatrix<MatrixXd> Mx(rows, cols, true);

    // Go through all the possible blocks.
    for (int cb = 0; cb < Mx.bCols(); cb++) {
        for (int rb = 0; rb < Mx.bRows(); rb++) {
            // This shouldn't allocate
            MatrixXd* B = Mx.block(rb, cb);
            ASSERT_EQ((MatrixXd*)NULL, B) << "Block at " << rb << "," << cb << " should be null";

            // This shouldn't allocate
            B = Mx.block(rb, cb, false);
            ASSERT_EQ((MatrixXd*)NULL, B) << "Block at " << rb << "," << cb << " should be null";

            // This should allocate.
            B = Mx.block(rb, cb, true);
            ASSERT_NE((MatrixXd*)NULL, B);
            ASSERT_EQ(B->rows(), Mx.rowsOfBlock(rb));
            ASSERT_EQ(B->cols(), Mx.colsOfBlock(cb));
        }
    }
}

TEST(sparse_block_matrixTestSuite, testOperator) {
    using namespace sparse_block_matrix;
    using namespace Eigen;

    VectorXi rows(2);
    rows << 2, 4;
    VectorXi cols(3);
    cols << 2, 5, 7;

    SparseBlockMatrix<MatrixXd> M(rows, cols, true);
    // Block rows and columns.
    ASSERT_EQ(2, M.bRows());
    ASSERT_EQ(3, M.bCols());
    // Actual rows and columns.
    ASSERT_EQ(4, M.rows());
    ASSERT_EQ(7, M.cols());

    for (int r = 0; r < M.rows(); r++) {
        for (int c = 0; c < M.cols(); c++) {
            ASSERT_EQ(0.0, M(r, c)) << "The uninitailzed matrix did not return zero at " << r << "," << c;
        }
    }

    // Now fill a block.
    SparseBlockMatrix<MatrixXd>::Block* B = M.block(1, 0, true);
    ASSERT_NE(B, (SparseBlockMatrix<MatrixXd>::Block*)NULL);
    ASSERT_EQ(B->rows(), 2);
    ASSERT_EQ(B->cols(), 2);
    (*B)(0, 0) = 1.0;
    (*B)(0, 1) = 2.0;
    (*B)(1, 0) = 3.0;
    (*B)(1, 1) = 4.0;

    double expected = 1.0;
    for (int r = 0; r < M.rows(); r++) {
        for (int c = 0; c < M.cols(); c++) {
            if (r >= 2 && r < 4 && c < 2) {
                // This is part of the block.
                EXPECT_NEAR(expected, M(r, c), 1e-16) << "Failure at " << r << "," << c;
                expected += 1.0;
            } else {
                ASSERT_EQ(0.0, M(r, c)) << "The uninitailzed matrix did not return zero at " << r << "," << c;
            }
        }
    }
}

TEST(sparse_block_matrixTestSuite, testClone) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(4);
    rows << 2, 4, 14, 15;
    VectorXi cols(3);
    cols << 3, 5, 7;

    // Create an empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> ME = buildRandomMatrix<MatrixXd>(rows, cols, -1.0);

    sparse_block_matrix::SparseBlockMatrix<MatrixXd>* ME2 = ME.clone();
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> ME3;
    ME.cloneInto(ME3);

    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(ME, ME3);
    }

    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(ME, (*ME2));
    }

    delete ME2;

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M1 = buildRandomMatrix<MatrixXd>(rows, cols, 0.5);

    sparse_block_matrix::SparseBlockMatrix<MatrixXd>* M2 = M1.clone();
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M3;
    M1.cloneInto(M3);

    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(M1, (*M2));
    }

    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(M1, M3);
    }

    delete M2;
}

TEST(sparse_block_matrixTestSuite, testToDense) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(4);
    rows << 2, 4, 14, 15;
    VectorXi cols(3);
    cols << 3, 5, 7;

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M1 = buildRandomMatrix<MatrixXd>(rows, cols, 0.5);

    Eigen::MatrixXd D1 = M1.toDense();
    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(M1, D1);
    }
}

// //! transposes a block matrix, The transposed type should match the argument false on failure
// template <class MatrixTransposedType>
// bool transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const;
TEST(sparse_block_matrixTestSuite, testTranspose) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(4);
    rows << 2, 4, 14, 15;
    VectorXi cols(3);
    cols << 3, 5, 7;

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M1 = buildRandomMatrix<MatrixXd>(rows, cols, 0.5);

    Eigen::MatrixXd D1t = M1.toDense().transpose();

    sparse_block_matrix::SparseBlockMatrix<MatrixXd>* M1t = NULL;
    // First the NULL version.
    ASSERT_TRUE(M1.transpose(M1t));
    ASSERT_NE((sparse_block_matrix::SparseBlockMatrix<MatrixXd>*)NULL, M1t);

    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual((*M1t), D1t);
    }
    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(M1.transpose(), D1t);
    }

    // Now the non null version.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M2t(M1t->rowBlockIndices(), M1t->colBlockIndices(), true);
    sparse_block_matrix::SparseBlockMatrix<MatrixXd>* M2tp = &M2t;
    ASSERT_TRUE(M1.transpose(M2tp));

    {
        SCOPED_TRACE("");
        sparse_block_matrix::expectEqual(M2t, D1t);
    }

    delete M1t;

    // What if there is cruff in the transpose matrix? This fails.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M3t = buildRandomMatrix<MatrixXd>(cols, rows, 0.5);
    sparse_block_matrix::SparseBlockMatrix<MatrixXd>* M3tp = &M2t;
    ASSERT_TRUE(M1.transpose(M3tp));

    {
        SCOPED_TRACE("");
        // Fails.
        // sparse_block_matrix::expectEqual( M3t.toDense(), D1t);
    }

    // test with statically sized matrices
    rows << 2, 4, 6, 8;
    cols << 1, 2, 3;
    sparse_block_matrix::SparseBlockMatrix<Matrix<double, 2, 1> > M3 =
        buildRandomMatrix<Matrix<double, 2, 1> >(rows, cols, 0.5);
    {
        SCOPED_TRACE("");
        sm::eigen::assertEqual(M3.transpose().toDense(), M3.toDense().transpose().eval(), SM_SOURCE_FILE_POS);
    }
}

TEST(sparse_block_matrixTestSuite, testEigenVectorMultiplication) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(4);
    rows << 2, 4, 14, 15;
    VectorXi cols(3);
    cols << 3, 5, 7;

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M1 = buildRandomMatrix<MatrixXd>(rows, cols, 0.5);

    // a vector to multiply
    VectorXd src = VectorXd::Random(7);
    VectorXd dstp(15);
    dstp.setZero();
    // multiply
    M1.multiply(&dstp, src);

    // existing impl.
    double* dstp2 = NULL;
    //
    M1.multiply(dstp2, &src[0]);

    // make a dense multiplicaation:
    Eigen::MatrixXd M1dense = M1.toDense();
    Eigen::MatrixXd dstDense = M1dense * src;

    sm::eigen::assertNear(dstp, dstDense, 1e-6, SM_SOURCE_FILE_POS);
    Eigen::Map<Eigen::VectorXd> dstp2Map(dstp2, dstp.rows());
    sm::eigen::assertNear(dstDense, dstp2Map, 1e-6, SM_SOURCE_FILE_POS);

    double src4[15];
    double* dstp4 = new double[15];
    for (int i = 0; i < 15; i++) dstp4[i] = 0;
    VectorXd src3 = VectorXd::Random(15);
    VectorXd dstp3(7);
    dstp3.setZero();

    M1.rightMultiply(&dstp3, src3);

    for (int i = 0; i < src3.rows(); i++) src4[i] = src3(i);

    // take product:

    M1.rightMultiply(dstp4, &src4[0]);

    Eigen::MatrixXd dstDense3 = src3.transpose() * M1dense;

    Eigen::Map<Eigen::VectorXd> dstp2Map2(dstp4, dstp3.rows());

    sm::eigen::assertNear(dstp3, dstDense3.transpose(), 1e-6, SM_SOURCE_FILE_POS);
    sm::eigen::assertNear(dstDense3.transpose(), dstp2Map2, 1e-6, SM_SOURCE_FILE_POS);

    delete[] dstp2;
    delete[] dstp4;
}

TEST(sparse_block_matrixTestSuite, testScaleMatrix) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(4);
    rows << 2, 4, 14, 15;
    VectorXi cols(3);
    cols << 3, 5, 7;

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M1 = buildRandomMatrix<MatrixXd>(rows, cols, 0.5);

    double scale = std::rand();

    try {
        sm::eigen::assertNear((M1 * scale).toDense(), (M1.toDense() * scale).eval(), 1e-6, SM_SOURCE_FILE_POS);
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}

TEST(sparse_block_matrixTestSuite, testMatrixMatrixMultiplication) {
    using namespace Eigen;
    using namespace sparse_block_matrix;
    VectorXi rows(4);
    rows << 2, 4, 14, 15;
    VectorXi cols(3);
    cols << 3, 5, 7;

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M1 = buildRandomMatrix<MatrixXd>(rows, cols, 0.5);

    // Create an not empty matrix.
    sparse_block_matrix::SparseBlockMatrix<MatrixXd> M2 = buildRandomMatrix<MatrixXd>(cols, rows, 0.5);

    try {
        sm::eigen::assertNear((M1 * M2).toDense(), (M1.toDense() * M2.toDense()).eval(), 1e-6, SM_SOURCE_FILE_POS);
    } catch (const std::exception& e) {
        FAIL() << e.what();
    }
}
// //! adds the current matrix to the destination
// bool add(SparseBlockMatrix<MatrixType>*& dest) const ;

// //! dest = (*this) *  src
// void multiply(double*& dest, const double* src) const;

// //! dest = M * (*this)
// void rightMultiply(double*& dest, const double* src) const;

// SparseBlockMatrix*  slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;


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

#include "sycomore/GridScanner.h"

sycomore::Stride compute_stride(sycomore::Shape const & shape)
{
    if(shape.size() == 0)
    {
        return sycomore::Stride();
    }

    sycomore::Stride stride(shape.size()+1);
    stride[0] = 1;
    for(unsigned int i=0; i< shape.size(); ++i)
    {
        stride[i+1] = shape[i]*stride[i];
    }

    return stride;
}

BOOST_AUTO_TEST_CASE(WithoutRegion)
{
    sycomore::Index const origin{-3, -5, -7};
    sycomore::Shape const shape{7, 11, 15};
    sycomore::Stride const stride(compute_stride(shape));

    std::vector<int> data(stride[stride.size()-1]);
    std::iota(data.begin(), data.end(), 0);

    sycomore::GridScanner const scanner(origin, shape);
    auto iterator = scanner.begin();

    for(int z=-7; z<=7; ++z)
    {
        for(int y=-5; y<=5; ++y)
        {
            for(int x=-3; x<=3; ++x)
            {
                BOOST_CHECK(iterator != scanner.end());

                auto && index = iterator->first;
                BOOST_CHECK(index == sycomore::Index({x,y, z}));

                auto && offset = iterator->second;
                BOOST_CHECK(offset == data[offset]);

                ++iterator;
            }
        }
    }
    BOOST_CHECK(iterator == scanner.end());
}

BOOST_AUTO_TEST_CASE(WithRegion3D)
{
    sycomore::Index const origin{-3, -5, -7};
    sycomore::Shape const shape{7, 11, 15};
    sycomore::Stride const stride(compute_stride(shape));

    std::vector<int> data(stride[stride.size()-1]);
    std::iota(data.begin(), data.end(), 0);

    sycomore::Index const region_origin{-1, -2, -3};
    sycomore::Shape const region_shape{5, 7, 11};

    sycomore::GridScanner const scanner(origin, shape, region_origin, region_shape);
    auto iterator = scanner.begin();

    for(int z=-3; z<=7; ++z)
    {
        for(int y=-2; y<=4; ++y)
        {
            for(int x=-1; x<=3; ++x)
            {
                BOOST_CHECK(iterator != scanner.end());

                auto && index = iterator->first;
                BOOST_CHECK(index == sycomore::Index({x,y, z}));

                auto && offset = iterator->second;
                BOOST_CHECK(offset == data[offset]);

                ++iterator;
            }
        }
    }
    BOOST_CHECK(iterator == scanner.end());

}

BOOST_AUTO_TEST_CASE(WithRegion2D)
{
    sycomore::Index const origin{-3, -5, -7};
    sycomore::Shape const shape{7, 11, 15};
    sycomore::Stride const stride(compute_stride(shape));

    std::vector<int> data(stride[stride.size()-1]);
    std::iota(data.begin(), data.end(), 0);

    sycomore::Index const region_origin{-1, -2, -3};
    sycomore::Shape const region_shape{5, 1, 11};

    sycomore::GridScanner const scanner(origin, shape, region_origin, region_shape);
    auto iterator = scanner.begin();

    for(int z=-3; z<=7; ++z)
    {
        for(int y=-2; y<=-2; ++y)
        {
            for(int x=-1; x<=3; ++x)
            {
                BOOST_CHECK(iterator != scanner.end());

                auto && index = iterator->first;
                BOOST_CHECK(index == sycomore::Index({x,y, z}));

                auto && offset = iterator->second;
                BOOST_CHECK(offset == data[offset]);

                ++iterator;
            }
        }
    }
    BOOST_CHECK(iterator == scanner.end());

}


// Copyright Aleksey Gurtovoy 2001-2004
//
// 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)
//
// See http://www.boost.org/libs/mpl for documentation.

// $Id: transform_view.cpp 49268 2008-10-11 06:26:17Z agurtovoy $
// $Date: 2008-10-11 02:26:17 -0400 (Sat, 11 Oct 2008) $
// $Revision: 49268 $

#include <boost/mpl/transform_view.hpp>

#include <boost/mpl/max_element.hpp>
#include <boost/mpl/list.hpp>
#include <boost/mpl/sizeof.hpp>
#include <boost/mpl/aux_/test.hpp>

MPL_TEST_CASE()
{
    typedef list<int,long,char,char[50],double> types;
    typedef max_element<
          transform_view< types, sizeof_<_> >
        >::type iter;

    MPL_ASSERT_RELATION( deref<iter>::type::value, ==, 50 );
}


//
// Copyright (c) 2021 Vinnie Falco (vinnie dot falco at gmail dot com)
//
// 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)
//
// Official repository: https://github.com/CPPAlliance/http_proto
//

#ifndef BOOST_HTTP_PROTO_RULE_IMPL_NUMBER_IPP
#define BOOST_HTTP_PROTO_RULE_IMPL_NUMBER_IPP

#include <boost/http_proto/bnf/number.hpp>
#include <boost/http_proto/error.hpp>
#include <boost/http_proto/bnf/ctype.hpp>
#include <boost/url/grammar/charset.hpp>

namespace boost {
namespace http_proto {
namespace bnf {

char const*
dec_number::
parse(
    char const* const start,
    char const* const end,
    error_code& ec)
{
    if(start == end)
    {
        ec = grammar::error::incomplete;
        return start;
    }
    auto ds = grammar::digit_chars;
    auto const max = (static_cast<
        std::uint64_t>(-1));
    auto const max10 = max / 10;
    auto it = start;
    v = 0;
    do
    {
        if(! ds(*it))
        {
            if(it == start)
            {
                // no digits
                ec = error::bad_number;
                return start;
            }
            break;
        }
        if(v > max10)
        {
            ec = error::numeric_overflow;
            return start;
        }
        v *= 10;
        std::uint64_t const d =
            *it - '0';
        if(max - v < d)
        {
            ec = error::numeric_overflow;
            return start;
        }
        v += d;
    }
    while(++it != end);
    return it;
}

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

char const*
hex_number::
parse(
    char const* const start,
    char const* const end,
    error_code& ec)
{
    if(start == end)
    {
        ec = grammar::error::incomplete;
        return start;
    }
    auto const hs = grammar::hexdig_chars;
    auto const max = (static_cast<
        std::uint64_t>(-1));
    auto const max16 = max / 16;
    auto it = start;
    v_ = 0;
    do
    {
        if(! hs(*it))
        {
            if(it == start)
            {
                // no digits
                ec = error::bad_number;
                return start;
            }
            break;
        }
        if(v_ > max16)
        {
            ec = error::numeric_overflow;
            return start;
        }
        v_ *= 16;
        char d0;
        grammar::hexdig_value(*it, d0);
        std::uint64_t const d = d0;
        if(max - v_ < d)
        {
            ec = error::numeric_overflow;
            return start;
        }
        v_ += d;
    }
    while(++it != end);
    return it;
}

} // bnf
} // http_proto
} // boost

#endif


#include <ros/node_handle.h>
#include <Eigen/Core>
#include <rosparam_utilities/rosparam_utilities.h>
#include <state_space_ros/ros_params.h>
#include <cnr_position_to_velocity_controller/cnr_position_to_velocity_math.h>
//#include <urdf/model.h>

namespace ect = eigen_control_toolbox;
namespace eu  = eigen_utils;
namespace ru  = rosparam_utilities;

namespace cnr
{
namespace control
{

bool append_string(std::string& what, bool ok, const std::string& msg)
{
  std::string ch   =  ok ? "[?]" : "[!]";
  std::string msg_ = msg.size()>0 ? ch + " " + msg : "";

  what += msg_.size()>0 ?
	  ( (what.size()>0 ? "\n" : "") + msg_ ) : "";
  return ok;
}

//!
bool PositionToVelocityControllerMath::init(
    ros::NodeHandle& ctrl_nh,const rosdyn::VectorXd& speed_limit,std::string& what)
{
  what = "";

  //======================= CHECK IF ELEMENT ARE RESIZABLE (STATIC vs DYN ALLOCATION, and check malloc)
  if(!eu::resize(m_pos_minimum_error, eu::size(speed_limit))
  || !eu::resize(m_pos_maximum_error, eu::size(speed_limit))
  || !eu::resize(m_last_target_pos  , eu::size(speed_limit))
  || !eu::resize(m_pos_deadband     , eu::size(speed_limit))
  || !eu::resize(m_antiwindup_gain  , eu::size(speed_limit), eu::size(speed_limit))
  || !eu::resize(m_pos_cmd          , eu::size(speed_limit))
  || !eu::resize(m_vel_cmd          , eu::size(speed_limit))
  || !eu::resize(m_eff_cmd          , eu::size(speed_limit))
  || !eu::resize(m_antiwindup       , eu::size(speed_limit))
  || !eu::resize(m_max_velocity     , eu::size(speed_limit)))
  {
    what += (what.size()>0? "\n[!] " : "[!] ") + std::string("Error in resize a vector");
    std::cout << what << std::endl;
    return false;
  }
  //=================================

  //================================= default values
  eu::setZero(m_pos_minimum_error);
  eu::setConstant(m_pos_maximum_error,0.1);
  eu::setZero(m_last_target_pos       );
  eu::setZero(m_pos_deadband          );
  eu::setDiagonal(m_antiwindup_gain, 1.0);
  eu::setZero(m_pos_cmd               );
  eu::setZero(m_vel_cmd               );
  eu::setZero(m_eff_cmd               );
  eu::setZero(m_antiwindup            );
  eu::setZero(m_max_velocity          );
  m_interpolate_setpoint = false;
  m_maximum_interpolation_time = 0.01;
  m_use_target_velocity = false;
  m_use_target_torque = false;
  //================================= default values

  // PARAM
  bool ok = true;
  std::string msg;
  std::vector<double> pos_minimum_error, pos_maximum_error,  antiwindup_gain;
  ok &= append_string( what,ru::getParam(ctrl_nh,"position_minimum_error"    , pos_minimum_error           , msg, &pos_minimum_error   ), msg);
  ok &= append_string( what,ru::getParam(ctrl_nh,"position_maximum_error"    , pos_maximum_error           , msg, &pos_maximum_error   ), msg);
  ok &= append_string( what,ru::getParam(ctrl_nh,"interpolate_setpoint"      , m_interpolate_setpoint      , msg, &m_interpolate_setpoint), msg) ;
  ok &= append_string( what,ru::getParam(ctrl_nh,"maximum_interpolation_time", m_maximum_interpolation_time, msg, &m_maximum_interpolation_time), msg);
  ok &= append_string( what,ru::getParam(ctrl_nh,"antiwindup_ratio"          , antiwindup_gain             , msg, &antiwindup_gain), msg);
  ok &= append_string( what,ru::getParam(ctrl_nh,"use_target_velocity"       , m_use_target_velocity       , msg, &m_use_target_velocity), msg);
  ok &= append_string( what,ru::getParam(ctrl_nh,"use_target_torque"         , m_use_target_torque         , msg, &m_use_target_torque), msg );
  ok &= append_string( what,ect::setMatricesFromParam(m_target_pos_filter  ,ctrl_nh, "target_pos_filter"   , msg), msg );
  ok &= append_string( what,ect::setMatricesFromParam(m_pos_filter         ,ctrl_nh, "pos_filter"          , msg), msg );
  ok &= append_string( what,ect::setMatricesFromParam(m_controller         ,ctrl_nh, "controller"          , msg), msg );
  ok &= append_string( what,ect::setMatricesFromParam(m_integral_controller,ctrl_nh, "integral_controller" , msg), msg );

  //=============

  std::vector<std::pair<int, std::string>> checks2 =
  {
    { m_target_pos_filter.uDim() == eu::size(speed_limit), "Wrong Target Pos Filter input dimension"},
    { m_target_pos_filter.yDim() == eu::size(speed_limit), "Wrong Target Pos Filter output dimension"},
    { m_pos_filter.uDim() == eu::size(speed_limit) ? 1 : -1, "Wrong Pos Filter input dimension"},
    { m_pos_filter.yDim() == eu::size(speed_limit) ? 1 : -1, "Wrong Pos Filter output dimension"},
    { m_controller.uDim() == eu::size(speed_limit) ? 1 : -1, "Wrong Controller input dimension"},
    { m_controller.yDim() == eu::size(speed_limit) ? 1 : -1, "Wrong Controller output dimension"},
    { m_integral_controller.uDim() == eu::size(speed_limit) ? 1 : -1, "Wrong Integral Controller input dimension"},
    { m_integral_controller.yDim() == eu::size(speed_limit) ? 1 : -1, "Wrong Integral Controller output dimension"},
    { eu::rows(pos_minimum_error) == eu::rows(m_pos_minimum_error) || eu::rows(pos_minimum_error)==1, "Wrong position_minimum_error dimension"},
    { eu::rows(pos_maximum_error) == eu::rows(m_pos_maximum_error) || eu::rows(pos_maximum_error)==1, "Wrong position_maximum_error dimension"},
    { eu::rows(antiwindup_gain)   == eu::rows(m_antiwindup_gain)   || eu::rows(antiwindup_gain)==1, "Wrong antiwindup_gain dimension, got"
      + std::to_string(eu::rows(antiwindup_gain)) +", expected 1 or " + std::to_string(eu::rows(m_antiwindup_gain)) },
  };

  for(size_t i=0;i<checks2.size();i++)
  {
    ok &= checks2.at(i).first;
    std::string ch = (checks2.at(i).first == -1) ? "[!]" : "[?]";
    what += checks2.at(i).first==1 ? "" : ( (what.size()>0? "\n" : "" ) + ch + " " + checks2.at(i).second );
  }
  if(!ok)
  {
    return false;
  }

  if(eu::rows(pos_minimum_error) == eu::rows(m_pos_minimum_error))
  {
    eu::copy(m_pos_minimum_error, pos_minimum_error);
  }
  else
  {
    eu::setConstant(m_pos_minimum_error, pos_minimum_error.at(0));
  }

  if(eu::rows(pos_maximum_error) == eu::rows(m_pos_maximum_error))
  {
    eu::copy(m_pos_maximum_error, pos_maximum_error);
  }
  else
  {
    eu::setConstant(m_pos_maximum_error, pos_maximum_error.at(0));
  }

  if(eu::rows(antiwindup_gain) == eu::rows(m_antiwindup_gain))
  {
    eu::setDiagonal(m_antiwindup_gain, antiwindup_gain);
  }
  else
  {
    eu::setDiagonal(m_antiwindup_gain, antiwindup_gain.at(0));
  }

  m_max_velocity = speed_limit;

  return true;

}

//!
bool PositionToVelocityControllerMath::starting(const rosdyn::VectorXd& fb_pos, const rosdyn::VectorXd& fb_vel, std::string& what)
{
  if(fb_pos.rows()!=m_last_target_pos.rows() || fb_vel.rows()!=m_last_target_pos.rows())
  {
    what = "The input args has incorrect dimension. fb pos: " + std::to_string(fb_pos.rows())+", fb vel: "+std::to_string(fb_vel.rows());
    what += " last pos: (" + std::to_string(m_last_target_pos.rows())+"x"+std::to_string(m_last_target_pos.cols())+")";
    return false;
  }
  if(m_pos_filter.uDim()!=fb_pos.rows() || m_target_pos_filter.uDim()!=fb_pos.rows())
  {
    what = "The input args has incorrect dimension. fb pos: " + std::to_string(fb_pos.rows())+", fb vel: "+std::to_string(fb_vel.rows());
    what += " pos filter: " + std::to_string(m_pos_filter.uDim()) + "  target pos filter: "+std::to_string( m_target_pos_filter.uDim())+")";
    return false;
  }
  if(m_pos_filter.yDim()!=fb_pos.rows() || m_target_pos_filter.yDim()!=fb_pos.rows())
  {
    what = "The input args has incorrect dimension. fb pos: " + std::to_string(fb_pos.rows())+", fb vel: "+std::to_string(fb_vel.rows());
    what += " pos filter: " + std::to_string(m_pos_filter.yDim()) + "  target pos filter: "+std::to_string( m_target_pos_filter.yDim())+")";
    return false;
  }

  rosdyn::VectorXd init_pos;
  init_pos = fb_pos;
  m_last_target_pos = fb_pos;
  m_pos_filter.setStateFromLastInput(init_pos);

  init_pos = fb_pos;
  m_target_pos_filter.setStateFromLastInput(init_pos);

  rosdyn::VectorXd init_vel;
  if (m_use_target_velocity)
  {
    init_vel = fb_vel - fb_vel;
  }
  else
  {
    init_vel = fb_vel;
  }
  rosdyn::VectorXd init_error=m_target_pos_filter.y() - m_pos_filter.y();

  m_controller.setStateFromLastIO(init_error, init_vel);
  m_integral_controller.setStateFromLastIO(init_error, init_vel); // TODO fix INITIALIZATION of two controllers
  eu::setZero(m_antiwindup);
  return true;
}

//!
void PositionToVelocityControllerMath::stopping()
{
  eu::setZero(m_vel_cmd);
  eu::setZero(m_eff_cmd);
}

//!
bool PositionToVelocityControllerMath::update(const ros::Time& time,
      const rosdyn::VectorXd* const trg_pos,
      const rosdyn::VectorXd* const trg_vel,
      const rosdyn::VectorXd* const trg_eff,
      const double* const last_sp_time,
      const rosdyn::VectorXd& fb_pos,
      const rosdyn::VectorXd& /*fb_vel*/)
{
  try
  {
    rosdyn::VectorXd zero_val;
    eu::resize(zero_val, eu::rows(fb_pos));
    eu::setZero(zero_val);

    rosdyn::VectorXd filter_output             = zero_val;
    rosdyn::VectorXd target_filter_output      = zero_val;
    rosdyn::VectorXd controller_input          = zero_val;
    rosdyn::VectorXd controller_output         = zero_val;
    rosdyn::VectorXd integral_controller_input = zero_val;
    rosdyn::VectorXd integral_controller_output= zero_val;

    filter_output = m_pos_filter.update(fb_pos);

    if(trg_pos)
    {
      rosdyn::VectorXd target_pos = (*trg_pos);
      if (m_interpolate_setpoint && ((time.toSec() - *last_sp_time) <= m_maximum_interpolation_time))
      {
        target_pos = m_last_target_pos + (*trg_vel) * (time.toSec() - *last_sp_time);
      }
      target_filter_output = m_target_pos_filter.update(target_pos);
      m_last_target_pos = target_filter_output;
    }
    else
    {
      target_filter_output = m_target_pos_filter.update(m_target_pos_filter.y());
    }

    controller_input = target_filter_output - filter_output; //controller error

    if(eu::norm(controller_input) > eu::norm(m_pos_maximum_error))
    {
      ROS_ERROR("Exceeded the position_maximum_error!");
      eu::setZero(m_vel_cmd);
      return false;
    }
    auto s = [](const std::string w, auto & v) { std::cout <<w << ": " << eu::rows(v) << "x" << eu::cols(v) << std::endl;};

    integral_controller_input = target_filter_output - filter_output + m_antiwindup_gain * m_antiwindup; //integral controller error

    controller_output = m_controller.update(controller_input);
    integral_controller_output = m_integral_controller.update(integral_controller_input);

    m_pos_cmd = target_filter_output;
    m_vel_cmd = controller_output + integral_controller_output + ( m_use_target_velocity && trg_vel ? *trg_vel : zero_val );
    m_eff_cmd = m_use_target_torque && trg_eff ? *trg_eff : zero_val;

    for(int i=0; i<eu::size(m_pos_cmd);i++)
    {
      if(eu::at(m_vel_cmd,i) > eu::at(m_max_velocity,i))
      {
        eu::at(m_antiwindup,i) = eu::at(m_max_velocity,i) - eu::at(m_vel_cmd,i);
        eu::at(m_vel_cmd,i) = eu::at(m_max_velocity,i);
      }
      else if (eu::at(m_vel_cmd,i) < -eu::at(m_max_velocity,i))
      {
        eu::at(m_antiwindup,i) = -eu::at(m_max_velocity,i) - eu::at(m_vel_cmd,i);
        eu::at(m_vel_cmd,i) = -eu::at(m_max_velocity,i);
      }
      else
      {
        eu::at(m_antiwindup,i) = 0;
      }
    }

  }
  catch (...)
  {
    eu::setZero(m_vel_cmd);
    return false;
  }
  return true;
}

}
}