#include <catch.hpp>
#include <Spirit/State.h>
#include <Spirit/System.h>
#include <Spirit/Simulation.h>
#include <Spirit/Configurations.h>
#include <Spirit/Hamiltonian.h>
#include <Spirit/Constants.h>
#include <Spirit/Parameters.h>
#include <data/State.hpp>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <iostream>
#include <iomanip>
#include <sstream>

TEST_CASE( "Larmor Precession","[physics]" )
{
    // Input file
    auto inputfile = "core/test/input/physics_larmor.cfg";

    // Create State
    auto state = std::shared_ptr<State>( State_Setup( inputfile ), State_Delete );

    // Choose method
    auto method = "LLG";

    // Solvers to be tested
    std::vector<const char *>  solvers{ "Heun", "Depondt", "SIB" };

    // Set up one the initial direction of the spin
    float init_direction[3] = { 1., 0., 0. };                // vec parallel to x-axis
    Configuration_Domain( state.get(), init_direction );     // set spin parallel to x-axis

    // Assure that the initial direction is set
    // (note: this pointer will stay valid throughout this test)
    auto direction = System_Get_Spin_Directions( state.get() );
    REQUIRE( direction[0] == 1 );
    REQUIRE( direction[1] == 0 );
    REQUIRE( direction[2] == 0 );

    // Make sure that mu_s is the same as the one define in input file
    float mu_s;
    Hamiltonian_Get_mu_s( state.get(), &mu_s );
    REQUIRE( mu_s == 2 );

    // Get the magnitude of the magnetic field ( it has only z-axis component )
    float B_mag;
    float normal[3];
    Hamiltonian_Get_Field( state.get(), &B_mag, normal );

    // Get time step of method
    scalar damping = 0.3;
    float tstep = Parameters_Get_LLG_Time_Step( state.get() );
    Parameters_Set_LLG_Damping( state.get(), damping );

    scalar dtg = tstep * Constants_gamma() / ( 1.0 + damping*damping );

    for( auto opt : solvers )
    {
        // Set spin parallel to x-axis
        Configuration_Domain( state.get(), init_direction );

        for( int i=0; i<100; i++ )
        {
            INFO( std::string( opt ) << " failed spin trajectory test at iteration " << i );

            // A single iteration
            Simulation_SingleShot( state.get(), method, opt );

            // Expected spin orientation
            // TODO: the step size should not be scaled by mu_s
            scalar phi_expected = mu_s * dtg * (i+1) * B_mag;
            scalar sz_expected  = std::tanh( mu_s * damping * dtg * (i+1) * B_mag );
            scalar rxy_expected = std::sqrt( 1-sz_expected*sz_expected );
            scalar sx_expected  = std::cos(phi_expected) * rxy_expected;

            REQUIRE( Approx(direction[0]) == sx_expected );
            REQUIRE( Approx(direction[2]) == sz_expected );
        }
    }
}

TEST_CASE( "Finite Differences", "[physics]" )
{
    // Hamiltonians to be tested
    std::vector<const char *>  hamiltonians{ "core/test/input/fd_pairs.cfg" };
                                             //"core/test/input/fd_neighbours",
                                             //"core/test/input/fd_gaussian.cfg"};
    for( auto ham: hamiltonians )
    {
        INFO( " Testing " << ham );

        // create state
        auto state = std::shared_ptr<State>( State_Setup( ham ), State_Delete );

        Configuration_Random( state.get() );

        auto& vf = *state->active_image->spins;
        auto grad = vectorfield( state->nos );
        auto grad_fd = vectorfield( state->nos );

        state->active_image->hamiltonian->Gradient_FD( vf, grad_fd );
        state->active_image->hamiltonian->Gradient( vf, grad );

        for( int i=0; i<state->nos; i++)
            REQUIRE( grad_fd[i].isApprox( grad[i] ) );

        auto hessian = MatrixX( 3*state->nos, 3*state->nos );
        auto hessian_fd = MatrixX( 3*state->nos, 3*state->nos );

        state->active_image->hamiltonian->Hessian_FD( vf, hessian_fd );
        state->active_image->hamiltonian->Hessian( vf, hessian );

        REQUIRE( hessian_fd.isApprox( hessian ) );
    }
}


#pragma once
#ifndef OPENGM_STRUCTSVM_HXX
#define OPENGM_STRUCTSVM_HXX

#include <cassert>
#include <ostream>
#include <vector>

#include <dlib/svm.h>
#include <dlib/optimization.h>
#include <dlib/matrix.h>

#include "opengm/opengm.hxx"
#include "pgmlink/ext_opengm/loss_hamming.hxx"
#include "pgmlink/ext_opengm/loglinearmodel.hxx"
#include "opengm/inference/lpcplex.hxx"
#include "opengm/operations/minimizer.hxx"

namespace opengm {

template<class LLM>
class StructSvmDlib
  : public dlib::structural_svm_problem<dlib::matrix<typename LLM::ValueType> > {
public:
  typedef LLM LoglinearModelType;

  StructSvmDlib(const std::vector<LoglinearModelType>& samples,
		const std::vector<std::vector<typename LoglinearModelType::LabelType> > labels)
    : samples_(samples), samples_and_loss_(samples), labels_(labels) {
    for(size_t i=0; i < samples.size(); ++i) {
      augmentWithLoss(samples_and_loss_[i], labels_[i]);
      LLM loss(samples_[i].space());
      augmentWithLoss(loss, labels_[i]);
      loss_.push_back(loss);
    }
  };

  void train(std::vector<typename LoglinearModelType::ValueType>& learned_weights);

  ///
  /// interface required by dlib
  ///
  typedef dlib::matrix<typename LLM::ValueType> matrix_type_dlib;
  typedef typename matrix_type_dlib::type scalar_type_dlib;
  typedef matrix_type_dlib feature_vector_type_dlib;

  virtual long get_num_dimensions () const;
  virtual long get_num_samples () const ;
  virtual void get_truth_joint_feature_vector ( long idx, feature_vector_type_dlib& psi ) const;
  virtual void separation_oracle (
				  const long idx,
				  const matrix_type_dlib& current_solution,
				  scalar_type_dlib& loss,
				  feature_vector_type_dlib& psi
				  ) const;

private:
  void augmentWithLoss( LoglinearModelType&, std::vector<typename LoglinearModelType::LabelType>& );

  mutable std::vector<LoglinearModelType> samples_;
  mutable std::vector<LoglinearModelType> samples_and_loss_;
  std::vector<LoglinearModelType> loss_;
  std::vector<std::vector<typename LoglinearModelType::LabelType> > labels_;
};

/******************/
/* implementation */
/******************/

template<class LLM>
void StructSvmDlib<LLM>::train(std::vector<typename LoglinearModelType::ValueType>& learned_weights) {
  learned_weights.clear();
  dlib::matrix<typename LLM::ValueType> weights;
  dlib::oca solver;
  solver.set_subproblem_epsilon(0.0000001);
  //solver.set_subproblem_max_iterations(100);
  std::cout << "\n";
  std::cout << "oca: subproblem epsilon: " << solver.get_subproblem_epsilon() << "\n";
  std::cout << "oca: subproblem max iterations: " << solver.get_subproblem_max_iterations() << "\n";

  this->be_verbose();
  solver( *this, weights);
  //assert(weights.size() == 2);
  assert(weights.nc() == 1);
  for(long i = 0; i< weights.nr(); ++i) {
    learned_weights.push_back(weights(0,i));
  }
}

template<class LLM>
long StructSvmDlib<LLM>::get_num_dimensions() const {
  return samples_.front().numberOfWeights();
}

template<class LLM>
long StructSvmDlib<LLM>::get_num_samples() const {
  return samples_.size();
}

template<class LLM>
void StructSvmDlib<LLM>::get_truth_joint_feature_vector( long idx, feature_vector_type_dlib& psi ) const {
  psi.set_size(this->get_num_dimensions(), 1);

  std::vector<typename LLM::ValueType> feats(samples_[idx].numberOfWeights());
  samples_[idx].weightedFeatureSums( labels_[idx], feats);
  for(long i = 0; i < psi.size(); ++i) {
    psi(i,0) = feats[i];
    std::cout << "true feature["<<i<<"]: " << psi(i,0) << "\n";
  }
}

template<class LLM>
void StructSvmDlib<LLM>::separation_oracle(const long idx,
					   const matrix_type_dlib& current_solution,
					   scalar_type_dlib& loss,
					   feature_vector_type_dlib& psi
					   ) const {
  std::cout << "\n";
  std::cout << "Entered separation oracle\n";
  std::vector<typename LLM::ValueType> weights;
  for(int i = 0; i < current_solution.nr(); ++i) {
    weights.push_back(-1*current_solution(i, 0));
    std::cout << "weights["<<i<<"]: " << current_solution(i,0) << "\n";
  }
  samples_and_loss_[idx].setWeights(weights);
  samples_[idx].setWeights(weights);

  typedef opengm::LPCplex<LLM, opengm::Minimizer> optimizer_type;
  typename optimizer_type::Parameter params;
  params.verbose_ = true;
  params.integerConstraint_ = true;
  params.epGap_ = 0.01;
  opengm::InferenceTermination status;
  optimizer_type inference = optimizer_type(samples_and_loss_[idx], params);
  status = inference.infer();
  if(status != opengm::NORMAL) {
    throw RuntimeError("GraphicalModel::infer(): optimizer terminated unnormally");
  }

  std::vector<typename LLM::LabelType> optimal;
  status = inference.arg(optimal);
  if(status != opengm::NORMAL) {
    throw RuntimeError("GraphicalModel::infer(): solution extraction terminated unnormally");
  }
  std::cout << "current[0]: " << optimal[0] << " label[0]: " << labels_[idx][0]  << "\n";
  std::cout << "current[1]: " << optimal[1] << " label[1]: " << labels_[idx][1]  << "\n";
  std::cout << "current[2]: " << optimal[2] << " label[2]: " << labels_[idx][2]  << "\n";
  std::cout << "current[3]: " << optimal[3] << " label[3]: " << labels_[idx][3]  << "\n";
  std::cout << "etc. etc.\n";

  std::vector<typename LLM::ValueType> feats(samples_and_loss_[idx].numberOfWeights());
  samples_and_loss_[idx].weightedFeatureSums( optimal, feats );
  psi.set_size(samples_[idx].numberOfWeights(), 1);
  for(long i = 0; i < psi.size(); ++i) {
    std::cout << "feature["<<i <<"]: " << feats[i] << "\n";
    psi(i,0) = feats[i];
  }

  loss = -1*loss_[idx].evaluate(optimal);
  std::cout << "ground truth energy for idx" << idx <<": "<< -1*samples_[idx].evaluate(labels_[idx])<<"\n";
  std::cout << "energy for idx" << idx <<": "<< -1*samples_[idx].evaluate(optimal)<<"\n";
  std::cout << "energy incl. loss for idx" << idx <<": "<< -1*samples_and_loss_[idx].evaluate(optimal)<<"\n";
  std::cout << "loss for idx "<< idx <<": " << loss << "\n";
  std::cout << "leaving oracle\n\n";
}

template<class LLM>
void StructSvmDlib<LLM>::augmentWithLoss( LLM& m, std::vector<typename LoglinearModelType::LabelType>& target_labels ) {
  typename LLM::IndexType lidx[] = {0};
  HammingFunction<typename LLM::ValueType, typename LLM::IndexType, typename LLM::LabelType> hf;
  for(typename LLM::IndexType i = 0; i < target_labels.size(); ++i) {
    lidx[0] = i;
    hf = HammingFunction<typename LLM::ValueType, typename LLM::IndexType, typename LLM::LabelType>(m.numberOfLabels(i), target_labels[i]);
    m.addFactor(m.addFunction(hf), lidx, lidx+1 );
  }
}

} // end namespace opengm

#endif


#include <iostream>
#include <Eigen/Core>
#include <npe.h>

npe_function(default_matches_1)
npe_arg(a, dense_double, dense_float)
npe_arg(b, npe_matches(a))
npe_default_arg(c, npe_matches(b))
npe_begin_code()

a(0, 0) = 2.0;

if (c.rows() != 0 || c.cols() != 0) {
  c(0, 0) = 2.0;
}

return std::make_tuple(npe::move(a), npe::move(b), npe::move(c));

npe_end_code()

npe_function(default_matches_2)
npe_arg(a, dense_double, dense_float)
npe_arg(b, npe_matches(a))
npe_default_arg(c, npe_matches(a))
npe_arg(d, dense_long, dense_int)
npe_default_arg(e, npe_matches(d))
npe_default_arg(f, npe_matches(e))
npe_begin_code()

d(0, 0) = 2;

if (e.rows() != 0 || e.cols() != 0) {
  e(0, 0) = 2;
}

if (f.rows() != 0 || f.cols() != 0) {
  f(0, 0) = 2;
}

return std::make_tuple(npe::move(d), npe::move(e), npe::move(f));

npe_end_code()

npe_function(default_matches_3)
npe_arg(a, sparse_double, sparse_float)
npe_arg(b, npe_matches(a))
npe_default_arg(c, npe_matches(a))
npe_arg(d, dense_long, dense_int)
npe_default_arg(e, npe_matches(d))
npe_default_arg(f, npe_matches(e))
npe_begin_code()

d(0, 0) = 2;

if (e.rows() != 0 || e.cols() != 0) {
  e(0, 0) = 2;
}

if (f.rows() != 0 || f.cols() != 0) {
  f(0, 0) = 2;
}
if (c.rows() != 0 || c.cols() != 0) {
  return std::make_tuple(npe::move(a), npe::move(b), npe::move(c));
} else {
  return std::make_tuple(npe::move(d), npe::move(e), npe::move(f));
}

npe_end_code()


///////////////////////////////////////////////////////////////
//  Copyright 2012 John Maddock. Distributed under the Boost
//  Software License, Version 1.0. (See accompanying file
//  LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt

#ifndef BOOST_MP_CPP_INT_CORE_HPP
#define BOOST_MP_CPP_INT_CORE_HPP

#include <boost/assert.hpp>
#include <boost/integer.hpp>
#include <boost/integer_traits.hpp>
#include <boost/mpl/if.hpp>
#include <boost/mpl/int.hpp>
#include <boost/static_assert.hpp>

namespace boost {
namespace multiprecision {

namespace detail {

//
// These traits calculate the largest type in the list
// [unsigned] boost::long_long_type, long, int, which has the specified number
// of bits.  Note that intN_t and boost::int_t<N> find the first
// member of the above list, not the last.  We want the last in the
// list to ensure that mixed arithmetic operations are as efficient
// as possible.
//
template <unsigned N> struct largest_signed_type {
  typedef typename mpl::if_c<
      1 + std::numeric_limits<boost::long_long_type>::digits == N,
      boost::long_long_type,
      typename mpl::if_c<
          1 + std::numeric_limits<long>::digits == N, long,
          typename mpl::if_c<1 + std::numeric_limits<int>::digits == N, int,
                             typename boost::int_t<N>::exact>::type>::type>::
      type type;
};

template <unsigned N> struct largest_unsigned_type {
  typedef typename mpl::if_c<
      std::numeric_limits<boost::ulong_long_type>::digits == N,
      boost::ulong_long_type,
      typename mpl::if_c<
          std::numeric_limits<unsigned long>::digits == N, unsigned long,
          typename mpl::if_c<
              std::numeric_limits<unsigned int>::digits == N, unsigned int,
              typename boost::uint_t<N>::exact>::type>::type>::type type;
};

} // namespace detail

#if defined(BOOST_HAS_INT128)

typedef detail::largest_unsigned_type<64>::type limb_type;
typedef detail::largest_signed_type<64>::type signed_limb_type;
typedef boost::uint128_type double_limb_type;
typedef boost::int128_type signed_double_limb_type;
static const limb_type max_block_10 = 1000000000000000000uLL;
static const limb_type digits_per_block_10 = 18;

inline BOOST_MP_CXX14_CONSTEXPR limb_type block_multiplier(unsigned count) {
#ifdef BOOST_NO_CXX14_CONSTEXPR
  static
#else
  constexpr
#endif
      const limb_type values[digits_per_block_10] = {10,
                                                     100,
                                                     1000,
                                                     10000,
                                                     100000,
                                                     1000000,
                                                     10000000,
                                                     100000000,
                                                     1000000000,
                                                     10000000000,
                                                     100000000000,
                                                     1000000000000,
                                                     10000000000000,
                                                     100000000000000,
                                                     1000000000000000,
                                                     10000000000000000,
                                                     100000000000000000,
                                                     1000000000000000000};
  BOOST_ASSERT(count < digits_per_block_10);
  return values[count];
}

// Can't do formatted IO on an __int128
#define BOOST_MP_NO_DOUBLE_LIMB_TYPE_IO

// Need to specialise integer_traits for __int128 as it's not a normal native
// type:
} // namespace multiprecision

template <>
class integer_traits<multiprecision::double_limb_type>
    : public std::numeric_limits<multiprecision::double_limb_type>,
      public detail::integer_traits_base<
          multiprecision::double_limb_type, 0,
          ~static_cast<multiprecision::double_limb_type>(0)> {};
template <>
class integer_traits<multiprecision::signed_double_limb_type>
    : public std::numeric_limits<multiprecision::signed_double_limb_type>,
      public detail::integer_traits_base<
          multiprecision::signed_double_limb_type,
          static_cast<multiprecision::signed_double_limb_type>(
              (static_cast<multiprecision::double_limb_type>(1) << 127)),
          static_cast<multiprecision::signed_double_limb_type>(
              ((~static_cast<multiprecision::double_limb_type>(0)) >> 1))> {};

namespace multiprecision {

#else

typedef detail::largest_unsigned_type<32>::type limb_type;
typedef detail::largest_signed_type<32>::type signed_limb_type;
typedef detail::largest_unsigned_type<64>::type double_limb_type;
typedef detail::largest_signed_type<64>::type signed_double_limb_type;
static const limb_type max_block_10 = 1000000000;
static const limb_type digits_per_block_10 = 9;

inline limb_type block_multiplier(unsigned count) {
#ifdef BOOST_NO_CXX14_CONSTEXPR
  static
#else
  constexpr
#endif
      const limb_type values[digits_per_block_10] = {
          10,      100,      1000,      10000,     100000,
          1000000, 10000000, 100000000, 1000000000};
  BOOST_ASSERT(count < digits_per_block_10);
  return values[count];
}

#endif

static const unsigned bits_per_limb = sizeof(limb_type) * CHAR_BIT;

template <class T>
inline BOOST_MP_CXX14_CONSTEXPR void minmax(const T &a, const T &b, T &aa,
                                            T &bb) {
  if (a < b) {
    aa = a;
    bb = b;
  } else {
    aa = b;
    bb = a;
  }
}

enum cpp_integer_type {
  signed_magnitude = 1,
  unsigned_magnitude = 0,
  signed_packed = 3,
  unsigned_packed = 2
};

enum cpp_int_check_type { checked = 1, unchecked = 0 };

} // namespace multiprecision
} // namespace boost

//
// Figure out whether to support user-defined-literals or not:
//
#if !defined(BOOST_NO_CXX11_VARIADIC_TEMPLATES) &&                             \
    !defined(BOOST_NO_CXX11_USER_DEFINED_LITERALS) &&                          \
    !defined(BOOST_NO_CXX11_CONSTEXPR)
#define BOOST_MP_USER_DEFINED_LITERALS
#endif

#endif // BOOST_MP_CPP_INT_CORE_HPP


#pragma once
#ifndef _INITIALIZER_HPP_
#define _INITIALIZER_HPP_

#include <list>
#include <set>
#include <vector>
#include <algorithm>
#include <string>
#include <cassert>

#include <Eigen/Dense>
#include <Eigen/StdVector>

#include <petsctime.h>

#include "Grid.hpp"
#include "Interface.hpp"
#include "BoundingBox.hpp"
#include "TriangleElement.hpp"
#include "Geometry.hpp"

#include "tictoc.hpp"
#include "utility.h"

/// compares two signed distances
/// the signed distance and also prepedicular distance is compared
void sdfMin(cDistance* in, cDistance* inout, int len, MPI_Datatype* dptr) {
	int i = 0;
	for(i = 0; i < len; ++i) {
		double ax  = fabs(in[i].signedDistance), ay = fabs(inout[i].signedDistance);
		double eps = my_eps;
		if ( fabs(ax - ay) < eps ) {
			if (in[i].pDistance > inout[i].pDistance) {
				inout[i] = in[i];
			}
		} else if (ax > ay) {
			inout[i] = in[i];
		}
	}
}

class Initializer {
public:
	Initializer() {};

	// This is where the magic happens and the initializator puts the data in
	template <typename type, int dim>
	void operator() (Geometry& geom, Interface<type, dim>& interface, int groupID, int nGroups);

private:
	// some states here and utility functions needed to do the jolb

	MPI_Datatype mpi_cDist_type;
	MPI_Op mpi_sdfmin;

	void createTypes();
	template <typename type, int dim>
	void initAll(const Grid<type, dim>& gr, Geometry& geom, double*** data_ptr);
	template <typename type, int dim>
	void initBoundaries_nlb(const Grid<type, dim>& gr, Geometry& geom, double*** data_ptr, int boundaries);
	template <typename type, int dim>
	void initBoundaries_lb(const Grid<type, dim>& gr, Geometry& geom, double*** data_ptr, int boundaries, int groupID, int nGroups);

	int selectBoundaries(const Box<double, 3>& geometryAABB, const Box<double, 3>& procAABB);

	template <typename type, int dim>
	void putDataToBoundaries(const Grid<type, dim>& gr, double*** data_ptr, cDistance* distanceData, int cellNum, int boundaries);

	template <typename type, int dim>
	void putLocalDataInside(const Grid<type, dim>& gr, double*** data_ptr, const std::vector<int>& localTriangles, const Geometry& geom, double*** perpendicualDistance);

};

///
/// Main routine
///

void Initializer::createTypes() {
	// TODO this should be rewritten as create type placed in cDistance and create minFcn
	// define according data type for cDistance in MPI
	const int    nitems = 2;
	int          blocklengths[2] = {1, 1};
	MPI_Datatype types[2] = {MPI_DOUBLE, MPI_DOUBLE};
	MPI_Aint     offsets[2];

	offsets[0] = offsetof(cDistance, signedDistance);
	offsets[1] = offsetof(cDistance, pDistance);

	MPI_Type_create_struct(nitems, blocklengths, offsets, types, &mpi_cDist_type);
	MPI_Type_commit(&mpi_cDist_type);
	// end type definition

	// commit custom mpi reduce operation
	MPI_Op_create((MPI_User_function *) sdfMin, 1, &mpi_sdfmin);
	// end custom reduce operation commit
}

template <typename type, int dim>
void Initializer::initAll(const Grid<type, dim>& gr, Geometry& geom, double*** data_ptr) {

	DM cda;
	Vec gc;
	DMDACoor3d ***coors;

	DMGetCoordinatesLocal(gr.getDA() ,&gc);
	DMGetCoordinateDM(gr.getDA(), &cda);
	DMDAVecGetArray(cda, gc, &coors);

	int       x, y, z, m, n, p;
	int       cellCounter = 0, gh = 1;

	DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);
	double data_ref = 0.0, data_acc = 0.0;
	for (int i = x; i < x+m; ++i) {
		for (int j = y; j < y+n; ++j) {
			for (int k = z; k < z+p; ++k) {
				data_acc =
						geom.computeDistance(
							Eigen::Vector3d(coors[k][j][i].x,
											coors[k][j][i].y,
											coors[k][j][i].z),
							true
						);

				data_ref = geom.computeDistance(
							Eigen::Vector3d(coors[k][j][i].x,
											coors[k][j][i].y,
											coors[k][j][i].z),
							false
						);

			}
		}
	}

}

template <typename type, int dim>
void Initializer::initBoundaries_nlb(const Grid<type, dim>& gr, Geometry& geom, double*** data_ptr, int boundaries) {

	DM cda;
	Vec gc;
	DMDACoor3d ***coors;

	DMGetCoordinatesLocal(gr.getDA() ,&gc);
	DMGetCoordinateDM(gr.getDA(), &cda);
	DMDAVecGetArray(cda, gc, &coors);

	int       x, y, z, m, n, p;
	int       cellCounter = 0, gh = 1;

	DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);

	int tmpSum = 0;
	if ( boundaries & 1 ) {
		tmpSum+= gh*n*p;
		// this means lets initialize left side
		for (int i = x; i < x+gh; ++i) {
			for (int j = y; j < y+n; ++j) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j][i] =
							geom.computeDistance(
								Eigen::Vector3d(coors[k][j][i].x,
												coors[k][j][i].y,
												coors[k][j][i].z),
								true
							);
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 2 ) {
		tmpSum += gh*n*p;
		// this means lets initialize right side
		for (int i = x+m-1; i > x+m-1-gh; --i) {
			for (int j = y; j < y+n; ++j) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j][i] =
							geom.computeDistance(
								Eigen::Vector3d(coors[k][j][i].x,
												coors[k][j][i].y,
												coors[k][j][i].z),
								true
							);
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 4 ) {
		tmpSum += gh*m*p;
		// this means initialize the bottom side
		for (int j = y; j < y+gh; ++j) {
			for (int i = x; i < x+m; ++i) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j][i] =
							geom.computeDistance(
								Eigen::Vector3d(coors[k][j][i].x,
												coors[k][j][i].y,
												coors[k][j][i].z),
								true
							);
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 8 ) {
		tmpSum += gh*m*p;
		// this means initialize the top side
		for (int j = y+n-1; j > y+n-1-gh; --j) {
			for (int i = x; i < x+m; ++i) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j][i] =
							geom.computeDistance(
								Eigen::Vector3d(coors[k][j][i].x,
												coors[k][j][i].y,
												coors[k][j][i].z),
								true
							);
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 16 ) {
		tmpSum += gh*m*n;
		// this means initialize the front side
		for (int k = z; k < z+gh; ++k) {
			for (int i = x; i < x+m; ++i) {
				for (int j = y; j < y+n; ++j) {
					data_ptr[k][j][i] =
							geom.computeDistance(
								Eigen::Vector3d(coors[k][j][i].x,
												coors[k][j][i].y,
												coors[k][j][i].z),
								true
							);
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 32 ) {
		tmpSum += gh*m*n;
		// this means initialize the far side
		for (int k = z+p-1; k > z+p-1-gh; --k) {
			for (int i = x; i < x+m; ++i) {
				for (int j = y; j < y+n; ++j) {
					data_ptr[k][j][i] =
							geom.computeDistance(
								Eigen::Vector3d(coors[k][j][i].x,
												coors[k][j][i].y,
												coors[k][j][i].z),
								true
							);
					cellCounter++;
				}
			}
		}
	}

}

/*
template <typename type, int dim>
void Initializer::initBoundaries_lb(const Grid<type, dim>& gr,
		Geometry& geom,
		double*** data_ptr,
		int boundaries,
		int groupID, int nGroups) {

	tictoc tick;
	int myWorldRank, nProcsInWorld;
	int myBegin, myEnd;   // local indices start, local indices stop
	int myBoundaries;
	MPI_Comm_rank(MPI_COMM_WORLD, &myWorldRank);
	MPI_Comm_size(MPI_COMM_WORLD, &nProcsInWorld);

	tick.tic();
	// prepare memory to store the list of all boundary cells
	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > boundaryCellList;
	boundaryCellList.reserve( nProcsInWorld * 4 * gr.getM(0)*gr.getM(1) );

	// this is where the results go (final results)
	std::vector<cDistance> vDistance;
	// create the fucking list
	for (int p = 0; p < nProcsInWorld; ++p) {
		// loop through the processes and get the proc bounding box
		Box<double, 3> procBB    = gr.getNodeSpan(p);
		int boundariesToInit     = selectBoundaries(geom.aabb, procBB);

		std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > l =
				gr.getNodeBoundaryCells(p, boundariesToInit);

		// ok I need to remember what part of the boundary is mine, otherwise I am screwed
		if (myWorldRank == p) {
			myBegin      = boundaryCellList.size();
			myEnd        = myBegin + l.size();
			myBoundaries = boundariesToInit;
		}

		boundaryCellList.insert( boundaryCellList.end(), l.begin(), l.end() );
	}

	tick.toc(myWorldRank, MAKECELLLIST, "Create boundary list");

	// number of cells in the boundary list for the WHOLE FUCKING BOUNDARY
	int nBCells = boundaryCellList.size();
	std::cout << "ncells: " << nBCells << std::endl;
	tick.tic();
	// loadbalance the problem.
	// first create groups for the problem
	MPI_Comm groupCommunicator = MPI_COMM_WORLD;
	int nProcsInGroup;
	int myGroupRank;
	if ( nGroups > 1 ) {
		// but if the number of groups is larger than one we need to create the right communicator.
		// luckily the group assigment is already sorted in setup phase.
		MPI_Comm_split(MPI_COMM_WORLD, groupID, myWorldRank, &groupCommunicator);
	}
	MPI_Comm_rank(groupCommunicator, &myGroupRank);
	MPI_Comm_size(groupCommunicator, &nProcsInGroup);
	// now each group will split its workload of boundary cells and start doing stuff

	int numberOfCellsPerTask     = geom.getNumberOfCellsPerDomainLocal();
	assert( numberOfCellsPerTask >= 0 );

	int max_numberOfCellsPerTask = -std::numeric_limits<int>::max();
	size_t totalCellsToFillTheGaps  = 0;
	int exchangedNumberOfCellsPerTask[nProcsInGroup];

	MPI_Allgather(&numberOfCellsPerTask, 1, MPI_INT, exchangedNumberOfCellsPerTask, nProcsInGroup, MPI_INT, groupCommunicator);

	max_numberOfCellsPerTask = *std::max_element(exchangedNumberOfCellsPerTask, exchangedNumberOfCellsPerTask + nProcsInGroup);
	assert( max_numberOfCellsPerTask >= 0 ); // maximum should not be negative

	double diffs[nProcsInGroup];// diffs is the number of cells going to be added as balance for
	for (int p = 0 ; p < nProcsInGroup; ++p) {
		diffs[p] = max_numberOfCellsPerTask - exchangedNumberOfCellsPerTask[p];
		assert( diffs[p] >= 0 );
		totalCellsToFillTheGaps += size_t(max_numberOfCellsPerTask - exchangedNumberOfCellsPerTask[p]);
		 if (myWorldRank == 0)
			 std::cout << "DIFFS : " << (size_t)diffs[p] << "; MAX : " << max_numberOfCellsPerTask << "; EXCHANGED: " << exchangedNumberOfCellsPerTask[p] << std::endl;
	}
	if (myWorldRank == 0)
		 std::cout << "SUM: " << totalCellsToFillTheGaps << std::endl;
	size_t tmpSum = 0; double pSum = 0.0;

	if ( totalCellsToFillTheGaps > 0 ) {
		for (int p = 0 ; p < nProcsInGroup; ++p) {
			pSum += diffs[p]/double(totalCellsToFillTheGaps);
			tmpSum += size_t(diffs[p]);

			 if (myWorldRank == 0)
				 std::cout << "PROC: " << p << " DIFF: " << double(diffs[p])  << " / " << double(totalCellsToFillTheGaps) << " - " << diffs[p]/double(totalCellsToFillTheGaps) << " - " << pSum
				 << " - " << tmpSum << std::endl;

			diffs[p] = double(diffs[p]) / double(totalCellsToFillTheGaps);
		}
		 if (myWorldRank == 0)
			 std::cout << "TMPSUM: " << tmpSum << std::endl;
		assert( tmpSum == totalCellsToFillTheGaps ); // the percentage is not allowed to be greater then 100%
	}

	int theRest = nBCells - totalCellsToFillTheGaps;
	if ( theRest < 0 ) {
		// to make sure that assigment of negative shit is not happening
		theRest                 = 0;
		totalCellsToFillTheGaps = nBCells;
	}

	int start = 0, finish = 0;
	int send_offsets[nProcsInGroup];
	int send_lens[nProcsInGroup];
	int debugSum = 0;

	for (int p = 0; p < nProcsInGroup; ++p) {
		start  = finish;

		finish = (p == nProcsInGroup-1) ? nBCells : start + int( diffs[p]*double(totalCellsToFillTheGaps) ) + int( double(theRest)/double(nProcsInGroup) );

		send_offsets[p] = start;
		send_lens[p]    = finish - start;
		debugSum += send_lens[p];
	}
	tick.toc(myWorldRank, SETUPGROUPS, "Setup groups");
	assert( debugSum <= nBCells );
	assert( send_lens[nProcsInGroup-1] + send_offsets[nProcsInGroup-1] == nBCells );

	tick.tic();
	// std::cout << "send_offsets = " << send_offsets[myGroupRank] << ", send_lens = " << send_lens[myGroupRank] << ", nBCells = " << nBCells << std::endl;
	assert( send_lens[myGroupRank] >= 0 );
	assert( send_offsets[myGroupRank] >= 0 );
	assert( (send_offsets[myGroupRank] + send_lens[myGroupRank]) <= nBCells );
//	std::vector<cDistance> localDistances; localDistances.reserve( send_lens[myGroupRank] );
	cDistance localDistances[ send_lens[myGroupRank] ];
	// compute the distance within the group

	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >::const_iterator begin_range = boundaryCellList.begin() + send_offsets[myGroupRank];
	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> >::const_iterator end_range = boundaryCellList.begin() + send_offsets[myGroupRank] + send_lens[myGroupRank];
	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > subList(begin_range, end_range);
	std::cout << "allocated for locality check" << std::endl;

	geom.computeDistance(subList, localDistances);
	tick.toc(myWorldRank, COMPUTEDIST, "Compute bval dist");

	boundaryCellList.clear();

	// put data together. That means call allgatherv and synchronize the shit.

	tick.tic();
	assert( nBCells > 0 );
	vDistance.resize( nBCells );
	MPI_Allgatherv(&localDistances[0], send_lens[myGroupRank], mpi_cDist_type,  &vDistance[0], send_lens, send_offsets, mpi_cDist_type, groupCommunicator);

	tick.toc(myWorldRank, ALLGATHER, "Allgatherv vector");
	// now the data is placed together in one nice vector
	std::cout << "dumping vDistance: " << std::endl;
	for (auto d: vDistance) {
		std::cout << d.signedDistance << ", ";
	}
	std::cout << std::endl;

	// synchronize the groups by reduce operation. This is only needed when you have more then one group

	tick.tic();
	if ( nGroups > 1 ) {
//		mpi::all_reduce(world, &vDistance[0], nBCells, &vDistance[0], sdfMinimum<cDistance>());
		MPI_Allreduce(&vDistance[0], &vDistance[0], nBCells, mpi_cDist_type, mpi_sdfmin, MPI_COMM_WORLD);
	}
	tick.toc(myWorldRank, REDUCE, "Reduce between groups");

	tick.tic();
	// step THREE put into the grid
	assert( myBegin >= 0 );
	assert( (myEnd-myBegin) < nBCells );
	putDataToBoundaries(gr, data_ptr, &vDistance[myBegin], (myEnd-myBegin), myBoundaries);

	vDistance.clear();
	tick.toc(myWorldRank, PUTTOBOUNDARIES, "Place to boundaries");

}
*/

template <typename type, int dim>
void Initializer::operator ()(Geometry& geom, Interface<type, dim>& interface, int groupID, int nGroups) {

	std::vector<Eigen::Vector3d, Eigen::aligned_allocator<Eigen::Vector3d> > boundaryCells;    // xyz coordinated to compute the distance to

	int                        myBoundaries;     // local boundaries to init

	MPI_Group worldGroup;
	MPI_Comm_group(MPI_COMM_WORLD, &worldGroup);
	// perpendicular distance computed towards the triangle, it is used when we are no longer sure, which normal shall we use

	double*** perpendicualDistance;

	// initialize the data

	Vec       localData = interface.getLocalData();
	double*** data_ptr;
	int       x, y, z, m, n, p;
	int       vecSize;

	VecGetLocalSize(localData, &vecSize);
	assert( vecSize > 0 );

	int myWorldRank;
	int nProcsInWorld;

	MPI_Comm_rank(MPI_COMM_WORLD, &myWorldRank);
	MPI_Comm_size(MPI_COMM_WORLD, &nProcsInWorld);

	const Grid<type, dim>& gr = interface.getGrid();

	DMDAVecGetArray(gr.getDA(), localData, &data_ptr);
	DMDAGetArray(gr.getDA(), PETSC_TRUE, &perpendicualDistance);
	DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);

	assert( vecSize == m*n*p );

	for (int k = z, pk = 0; k < z+p; ++k, ++pk) {
		for (int j = y, pj = 0; j < y+n; ++j, ++pj) {
			for (int i = x, pi = 0; i < x+m; ++i, ++pi) {
				data_ptr[k][j][i]             =  std::numeric_limits<double>::max();
				perpendicualDistance[k][j][i] = -std::numeric_limits<double>::max();
			}
		}
	}

	// step FIVE compute triangle distance inside the narrowband

	putLocalDataInside(gr, data_ptr, geom.localElements, geom, perpendicualDistance);

	DMDARestoreArray(gr.getDA(), PETSC_TRUE, &perpendicualDistance);

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

	if ( nProcsInWorld == 1 ) {
//		DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);
//		for (int k = z; k < z+p; ++k) {
//			for (int j = y; j < y+n; ++j) {
//		 		for (int i = x; i < x+m; ++i) {
//		 			if ( data_ptr[k][j][i] >= 10E+5 )
//		 				data_ptr[k][j][i] = -10;
//				}
//			}
//		}
		DMDAVecRestoreArray(gr.getDA(), localData, &data_ptr);
		return;
	}

	int boundariesToInit = selectBoundaries(geom.aabb, gr.getNodeSpan(myWorldRank));
	initBoundaries_nlb(gr, geom, data_ptr, boundariesToInit);
	// initAll(gr, geom, data_ptr);

//=======================================================================================
//	  DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);
//	  for (int k = z; k < z+p; ++k) {
//	  	for (int j = y; j < y+n; ++j) {
//	  		for (int i = x; i < x+m; ++i) {
//	  			if ( data_ptr[k][j][i] >= 10E+5 ) {
//	  				data_ptr[k][j][i] = -10;
//	  			}
//	  		}
//	  	}
//	  }
//=======================================================================================

	DMDAVecRestoreArray(gr.getDA(), localData, &data_ptr);

}

int Initializer::selectBoundaries(const Box<double, 3>& geometryAABB, const Box<double, 3>& procAABB) {
	// chooses which boundaries of the domain have to be initialized
	// ordering : [left, right, bottom, top, front, back]

	int selectedBoundaries_int = 0;

	for (int i = 0; i < 3; ++i) {
		//gr->getLocalGhostedMax(i) <= dataProvider->getMaxX(i)
		if ( procAABB.maxX(i) <= geometryAABB.maxX(i) ) {
			selectedBoundaries_int |= ( 1 << (i*2 + 1) );
		}
		// gr->getLocalGhostedMin(i) >= dataProvider->getMinX(i)
		if ( procAABB.minX(i) >= geometryAABB.minX(i) ) {
			selectedBoundaries_int |= ( 1 << (i*2) );
		}
	}

	return selectedBoundaries_int;

}

template <typename type, int dim>
void Initializer::putDataToBoundaries(const Grid<type, dim>& gr, double*** data_ptr, cDistance* distanceData, int cellNum, int boundaries) {

	int       x, y, z, m, n, p;
	int       cellCounter = 0, gh = 1;

	DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);
//	DMDAGetCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);
	int tmpSum = 0;
	if ( boundaries & 1 ) {
		tmpSum+= gh*n*p;
		// this means lets initialize left side
		for (int i = x; i < x+gh; ++i) {
			for (int j = y; j < y+n; ++j) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j][i+1] = distanceData[cellCounter].signedDistance;
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 2 ) {
		tmpSum += gh*n*p;
		// this means lets initialize right side
		for (int i = x+m-1; i > x+m-1-gh; --i) {
			for (int j = y; j < y+n; ++j) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j][i-1] = distanceData[cellCounter].signedDistance;
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 4 ) {
		tmpSum += gh*m*p;
		// this means initialize the bottom side
		for (int j = y; j < y+gh; ++j) {
			for (int i = x; i < x+m; ++i) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j+1][i] = distanceData[cellCounter].signedDistance;
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 8 ) {
		tmpSum += gh*m*p;
		// this means initialize the top side
		for (int j = y+n-1; j > y+n-1-gh; --j) {
			for (int i = x; i < x+m; ++i) {
				for (int k = z; k < z+p; ++k) {
					data_ptr[k][j-1][i] = distanceData[cellCounter].signedDistance;
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 16 ) {
		tmpSum += gh*m*n;
		// this means initialize the front side
		for (int k = z; k < z+gh; ++k) {
			for (int i = x; i < x+m; ++i) {
				for (int j = y; j < y+n; ++j) {
					data_ptr[k+1][j][i] = distanceData[cellCounter].signedDistance;
					cellCounter++;
				}
			}
		}
	}
	if ( boundaries & 32 ) {
		tmpSum += gh*m*n;
		// this means initialize the far side
		for (int k = z+p-1; k > z+p-1-gh; --k) {
			for (int i = x; i < x+m; ++i) {
				for (int j = y; j < y+n; ++j) {
					data_ptr[k-1][j][i] = distanceData[cellCounter].signedDistance;
					cellCounter++;
				}
			}
		}
	}

	assert( cellNum == tmpSum );

}

template <typename type, int dim>
void Initializer::putLocalDataInside(const Grid<type, dim>& gr, double*** data_ptr, const std::vector<int>& localTriangles, const Geometry& geom, double*** perpendicualDistance) {
	int       x, y, z, m, n, p;
	double    narrowBand = gr.getDx(0) * 3;
	double    minDX      = Eigen::Array3d(gr.getDx(0), gr.getDx(1), gr.getDx(2)).minCoeff();
	double    eps        = my_eps;

	DMDAGetGhostCorners(gr.getDA(), &x, &y, &z, &m, &n, &p);

//	 std::cout << "number of triangles = " << localTriangles.size() << std::endl;
//	 for (auto i: localTriangles) {
//		 std::cout << i << ", ";
//	 }
//	 std::cout << std::endl;

	if ( localTriangles.size() == 0 )
		return;

	for (auto index: localTriangles) {
		TriangleElement<double> currentElement = geom.getElement(index);

//		std::cout << currentElement << std::endl;
//		std::cout << currentElement.getBoundingBox() << std::endl;

		Box<int, 3> ind = gr.getGlobalBoxIndices( currentElement.getBoundingBox() );

//		std::cout << ind << std::endl;

		auto bl = ind.bl();
		auto tr = ind.tr();

		for ( int k = bl[2]; k <= tr[2]; ++k ) {

			if ( k < z || k >= (z+p) )
				continue;

			for ( int j = bl[1]; j <= tr[1]; ++j ) {

				if ( j < y || j >= (y+n) )
					continue;

				for ( int i = bl[0]; i <= tr[0]; ++i ) {

					if ( i < x || i >= (x+m) )
						continue;

					Eigen::Vector3d        p       = gr.getCoord( Eigen::Vector3i(i,j,k) );

					SignedDistance<double> v       = TriangleElement<double>::computeDistance(currentElement, p);
					double                 pd      = TriangleElement<double>::computePerpendicularDistance(currentElement, p);

					if ( v.dist > narrowBand ) {
						continue;
					}

					if ( v.dist < eps ) {

						data_ptr[k][j][i]             = eps * v.sign;
						perpendicualDistance[k][j][i] = eps;

					}  else if ( fabs( v.dist - fabs(data_ptr[k][j][i]) ) < eps ) {

						// equal not funny
						if ( perpendicualDistance[k][j][i] <= pd ) {
							data_ptr[k][j][i]             = v.sign * v.dist;
							perpendicualDistance[k][j][i] = pd;
						}

					} else if ( v.dist < fabs(data_ptr[k][j][i]) ) {

						// less so ok than
						data_ptr[k][j][i]             = v.sign * v.dist;
						perpendicualDistance[k][j][i] = pd;
					}

				}
			}
		}

	}

}

#endif


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

#include <boost/test/unit_test.hpp>
#include "ParserFlatbuffersFixture.hpp"
#include "../TfLiteParser.hpp"

#include <string>
#include <iostream>

BOOST_AUTO_TEST_SUITE(TensorflowLiteParser)

    struct QuantizeFixture : public ParserFlatbuffersFixture
    {
        explicit QuantizeFixture(const std::string & inputShape,
                                 const std::string & outputShape,
                                 const std::string & dataType)
        {
            m_JsonString = R"(
            {
                "version": 3,
                "operator_codes": [ { "builtin_code": "QUANTIZE" } ],
                "subgraphs": [ {
                    "tensors": [
                        {
                            "shape": )" + inputShape + R"(,
                            "type": "FLOAT32",
                            "buffer": 0,
                            "name": "inputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.0 ],
                                "zero_point": [ 0 ],
                            }
                        },
                        {
                            "shape": )" + outputShape + R"( ,
                            "type": )" + dataType + R"(,
                            "buffer": 1,
                            "name": "outputTensor",
                            "quantization": {
                                "min": [ 0.0 ],
                                "max": [ 255.0 ],
                                "scale": [ 1.5 ],
                                "zero_point": [ 0 ],
                            }
                        }
                    ],
                    "inputs": [ 0 ],
                    "outputs": [ 1 ],
                    "operators": [
                        {
                            "opcode_index": 0,
                            "inputs": [ 0 ],
                            "outputs": [ 1 ],
                            "builtin_options_type": "QuantizeOptions",
                            "builtin_options": {
                            },
                            "custom_options_format": "FLEXBUFFERS"
                        }
                ],
                } ],
                "buffers" : [
                    { },
                    { },
                ]
            }
        )";
            SetupSingleInputSingleOutput("inputTensor", "outputTensor");
        }
    };

    struct SimpleQuantizeFixtureQAsymm8 : QuantizeFixture
    {
        SimpleQuantizeFixtureQAsymm8() : QuantizeFixture("[ 1, 6 ]",
                                                         "[ 1, 6 ]",
                                                         "UINT8") {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleQuantizeQAsymm8, SimpleQuantizeFixtureQAsymm8)
    {
        RunTest<2, armnn::DataType::Float32, armnn::DataType::QuantisedAsymm8>(
                0,
                {{"inputTensor",  { 0.0f, 1.5f, 7.5f, 150.0f, 300.0f, 382.5f }}},
                {{"outputTensor", { 0u,   1u,   5u,   100u,   200u,   255u }}});
    }

    struct SimpleQuantizeFixtureQSymm16 : QuantizeFixture
    {
        SimpleQuantizeFixtureQSymm16() : QuantizeFixture("[ 1, 6 ]",
                                                         "[ 1, 6 ]",
                                                         "INT16") {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleQuantizeQsymm16, SimpleQuantizeFixtureQSymm16)
    {
        RunTest<2, armnn::DataType::Float32, armnn::DataType::QuantisedSymm16>(
                0,
                {{"inputTensor",  { 0.0f, 1.5f, 7.5f, 49150.5f, -1.5f,-49152.0f }}},
                {{"outputTensor", { 0,    1,    5,    32767,    -1,   -32768 }}});
    }

    struct SimpleQuantizeFixtureQSymmS8 : QuantizeFixture
    {
        SimpleQuantizeFixtureQSymmS8() : QuantizeFixture("[ 1, 6 ]",
                                                         "[ 1, 6 ]",
                                                         "INT8") {}
    };

    BOOST_FIXTURE_TEST_CASE(SimpleQuantizeQSymmS8, SimpleQuantizeFixtureQSymmS8)
    {
        RunTest<2, armnn::DataType::Float32, armnn::DataType::QSymmS8>(
                0,
                {{"inputTensor",  { 0.0f, 1.5f, 7.5f, 190.5f, -192.0f, -1.5f }}},
                {{"outputTensor", { 0,    1,    5,    127,    -128,   -1 }}});
    }

BOOST_AUTO_TEST_SUITE_END()


/*
 * L2RightHandSide.cc
 *
 *  Created on: 29.06.2017
 *      Author: thies
 */

#include <deal.II/base/exceptions.h>
#include <deal.II/base/work_stream.h>
#include <deal.II/fe/fe_update_flags.h>
#include <deal.II/numerics/vector_tools.h>

#include <base/DiscretizedFunction.h>
#include <forward/L2RightHandSide.h>

#include <functional>

namespace wavepi {
namespace forward {
using namespace dealii;
using namespace wavepi::base;

template <int dim>
L2RightHandSide<dim>::L2RightHandSide(std::shared_ptr<Function<dim>> f) : base_rhs(f) {}

template <int dim>
L2RightHandSide<dim>::AssemblyScratchData::AssemblyScratchData(const FiniteElement<dim> &fe,
                                                               const Quadrature<dim> &quad)
    : fe_values(fe, quad, update_values | update_quadrature_points | update_JxW_values) {}

template <int dim>
L2RightHandSide<dim>::AssemblyScratchData::AssemblyScratchData(const AssemblyScratchData &scratch_data)
    : fe_values(scratch_data.fe_values.get_fe(), scratch_data.fe_values.get_quadrature(),
                update_values | update_quadrature_points | update_JxW_values) {}

template <int dim>
void L2RightHandSide<dim>::copy_local_to_global(Vector<double> &result, const AssemblyCopyData &copy_data) {
  for (unsigned int i = 0; i < copy_data.local_dof_indices.size(); ++i)
    result(copy_data.local_dof_indices[i]) += copy_data.cell_rhs(i);
}

template <int dim>
std::shared_ptr<Function<dim>> L2RightHandSide<dim>::get_base_rhs() const {
  return base_rhs;
}

template <int dim>
void L2RightHandSide<dim>::set_base_rhs(std::shared_ptr<Function<dim>> base_rhs) {
  this->base_rhs = base_rhs;
}

template <int dim>
void L2RightHandSide<dim>::local_assemble(const Vector<double> &f,
                                          const typename DoFHandler<dim>::active_cell_iterator &cell,
                                          AssemblyScratchData &scratch_data, AssemblyCopyData &copy_data) {
  const unsigned int dofs_per_cell = scratch_data.fe_values.get_fe().dofs_per_cell;
  const unsigned int n_q_points    = scratch_data.fe_values.get_quadrature().size();

  copy_data.cell_rhs.reinit(dofs_per_cell);
  copy_data.local_dof_indices.resize(dofs_per_cell);
  scratch_data.fe_values.reinit(cell);

  cell->get_dof_indices(copy_data.local_dof_indices);

  for (unsigned int q_point = 0; q_point < n_q_points; ++q_point)
    for (unsigned int i = 0; i < dofs_per_cell; ++i)
      for (unsigned int k = 0; k < dofs_per_cell; ++k)
        copy_data.cell_rhs(i) += f[copy_data.local_dof_indices[k]] * scratch_data.fe_values.shape_value(k, q_point) *
                                 scratch_data.fe_values.shape_value(i, q_point) * scratch_data.fe_values.JxW(q_point);
}

template <int dim>
void L2RightHandSide<dim>::create_right_hand_side(const DoFHandler<dim> &dof, const Quadrature<dim> &quad,
                                                  Vector<double> &rhs) const {
  AssertThrow(base_rhs, ExcInternalError());
  base_rhs->set_time(this->get_time());

  auto base_rhs_d = std::dynamic_pointer_cast<DiscretizedFunction<dim>>(base_rhs);

  if (base_rhs_d) {
    Vector<double> coeffs = base_rhs_d->get_function_coefficients(base_rhs_d->get_time_index());
    Assert(coeffs.size() == dof.n_dofs(), ExcDimensionMismatch(coeffs.size(), dof.n_dofs()));

    WorkStream::run(dof.begin_active(), dof.end(),
                    std::bind(&L2RightHandSide<dim>::local_assemble, *this, std::ref(coeffs), std::placeholders::_1,
                              std::placeholders::_2, std::placeholders::_3),
                    std::bind(&L2RightHandSide<dim>::copy_local_to_global, *this, std::ref(rhs), std::placeholders::_1),
                    AssemblyScratchData(dof.get_fe(), quad), AssemblyCopyData());
  } else
    VectorTools::create_right_hand_side(dof, quad, *base_rhs.get(), rhs);
}

template class L2RightHandSide<1>;
template class L2RightHandSide<2>;
template class L2RightHandSide<3>;

} /* namespace forward */
} /* namespace wavepi */


/******************************************************************************
 * test.cpp: Main Process.
 *
 * Author: Carlos Eduardo de Andrade <ce.andrade@gmail.com>
 *
 * (c) Copyright 2011-2018, Carlos Eduardo de Andrade. All Rights Reserved.
 *
 * Created on : Jun 16, 2012 by andrade
 * Last update: Jun 16, 2012 by andrade
 *
 * This code is released under LICENSE.md.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *****************************************************************************/

#include <iostream>
#include <fstream>
#include <iomanip>
#include <stdexcept>
#include <algorithm>
#include <iterator>
#include <limits>
#include <cstdlib>
#include <vector>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>
#include <lemon/full_graph.h>
#include <lemon/lgf_writer.h>
#include <lemon/dim2.h>

using namespace std;
using namespace lemon;

#include "concorde_wrapper.hpp"

double printCycle(const vector<FullGraph::Node> &cycle, const FullGraph &graph,
                  const FullGraph::EdgeMap<int> &dist);

//-------------------------------[ Main ]------------------------------------//

int main(int argc, char* argv[]) {
    if(argc < 2) {
        cerr << "> Please, give the TSP instance file" << endl;
        return 1;
    }

    FullGraph graph;
    FullGraph::EdgeMap<int> dist(graph);
    FullGraph::NodeMap< dim2::Point<double> > coords(graph);
    bool loaded_coords;

    try {
        const unsigned long SEED = 152269023;
        const unsigned long MAX_THR = 2;

        ConcordeWrapper concorde(SEED, MAX_THR);
        concorde.loadTSP(argv[1], &graph, &dist, &coords, &loaded_coords);
        concorde.updateData(&graph, &dist);

        if(loaded_coords)
            for(FullGraph::NodeIt it(graph); it != INVALID; ++it)
                cout << "\n> " << graph.id(it) << ": "<< coords[it];
        else
            cout << "\nWe couldn't load the geometric information" << endl;

        vector<FullGraph::Node> nodes;
        vector<FullGraph::Node> *cycle = new vector<FullGraph::Node>();

        for(FullGraph::NodeIt it(graph); it != INVALID; ++it)
            nodes.push_back(it);

        concorde.getLinKernTour(nodes, cycle);

        double cost = 0.0;
        cout << "\n>> Cycle: ";
        cost = printCycle(nodes, graph, dist);
        cout << "\n|| Cost: " << cost << endl;

        delete cycle;
    }
    catch(exception& e) {
        cerr << "\n***********************************************************"
             << "\n****  Exception Occured: " << e.what()
             << "\n***********************************************************"
             << endl;
        return 70; // BSD software internal error code
    }
    return 0;
}

//----------------------------[ Print Cycle ]---------------------------------//

double printCycle(const vector<FullGraph::Node> &cycle, const FullGraph &graph,
                  const FullGraph::EdgeMap<int> &dist) {
    double cost = 0.0;
    vector<FullGraph::Node>::const_iterator it1, it2;

    for(it2 = cycle.begin(); it2 != cycle.end(); ) {
        it1 = it2++;
        cout << graph.id(*it1) << " ";

        if(it2 != cycle.end())
            cost += dist[graph.edge(*it1,*it2)];
    }
    cost += dist[graph.edge(*it1,*(cycle.begin()))];

    return cost;
}


/*
 * MIT License
 *
 * Copyright (c) 2020 Jenna Reher (jreher@caltech.edu)
 *
 * 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 <cassie_description/cassie_model.hpp>
#include <ros/ros.h>
#include <ros/package.h>
#include <rbdl/addons/urdfreader/urdfreader.h>
#include <Eigen/Geometry>
#include <fstream>

using namespace RigidBodyDynamics;
using namespace Eigen;

namespace cassie_model {

void BodyPoint::initialize(Model *model, std::string point_name, std::string body_name, Eigen::Vector3d &point_position) {
    this->body_name = body_name;
    this->point_name = point_name;
    this->point_position << point_position;
    this->body_id = model->GetBodyId(body_name.c_str());
} // BodyPoint::initialize

Kinematics::Kinematics() {
    this->cache.init();
}

void Kinematics::Cache::init() {
    this->J_positionLeft.resize(3,22);
    this->J_positionRight.resize(3,22);
    this->J_leftLegLength.resize(1,7);
    this->J_rightLegLength.resize(1,7);
    this->J_poseLeft.resize(6,22);
    this->J_poseRight.resize(6,22);
    this->Jdot_poseLeft.resize(6,22);
    this->Jdot_poseRight.resize(6,22);

    this->J_poseLeftConstraint.resize(5,22);
    this->J_poseRightConstraint.resize(5,22);
    this->Jdot_poseLeftConstraint.resize(5,22);
    this->Jdot_poseRightConstraint.resize(5,22);
    this->J_achilles.resize(2,22);
    this->J_rigid.resize(4,22);
    this->Jdot_achilles.resize(2,22);
    this->Jdot_rigid.resize(4,22);
}

void Kinematics::initialize(Model *model) {
    Vector3d left_midfoot_body_frame_position = Vector3d(0,0,0);
    this->LeftMidFoot_Point.initialize(model, "LeftMidFoot", "leftfoot_link", left_midfoot_body_frame_position);

    Vector3d right_midfoot_body_frame_position = Vector3d(0,0,0);
    this->RightMidFoot_Point.initialize(model, "RightMidFoot", "rightfoot_link", right_midfoot_body_frame_position);

    Vector3d left_heelspringend_body_frame_position = Vector3d(0,0,0);
    this->LeftHeel_Point.initialize(model, "LeftSpringEnd", "leftheelspring_link", left_heelspringend_body_frame_position);

    Vector3d rightHeelspringend_body_frame_position = Vector3d(0,0,0);
    this->RightHeel_Point.initialize(model, "RightSpringEnd", "rightheelspring_link", rightHeelspringend_body_frame_position);
} // Kinematics::initialize

void Kinematics::update(Model *model, const VectorXd &q, const VectorXd &dq) {
    //RigidBodyDynamics::UpdateKinematicsCustom(*model, &q, &dq, nullptr);
    this->cache.J_achilles.setZero();
    SymFunction::J_achilles_constraint(this->cache.J_achilles, q);
    this->cache.J_poseLeft.setZero();
    SymFunction::J_leftSole_constraint(this->cache.J_poseLeftConstraint, q);
    this->cache.J_poseRight.setZero();
    SymFunction::J_rightSole_constraint(this->cache.J_poseRightConstraint, q);
    this->cache.Jdot_achilles.setZero();
    SymFunction::Jdot_achilles_constraint(this->cache.Jdot_achilles, q, dq);
    this->cache.Jdot_poseLeft.setZero();
    SymFunction::Jdot_leftSole_constraint(this->cache.Jdot_poseLeftConstraint, q, dq);
    this->cache.Jdot_poseRight.setZero();
    SymFunction::Jdot_rightSole_constraint(this->cache.Jdot_poseRightConstraint, q, dq);

    MatrixXd temp_left(2,22), temp_right(2,22);
    temp_left.setZero(); temp_right.setZero();
    SymFunction::J_left_fixed_constraint(temp_left, q);
    SymFunction::J_right_fixed_constraint(temp_right, q);
    this->cache.J_rigid << temp_left, temp_right;

    temp_left.setZero(); temp_right.setZero();
    SymFunction::Jdot_left_fixed_constraint(temp_left, q, dq);
    SymFunction::Jdot_right_fixed_constraint(temp_right, q, dq);
    this->cache.Jdot_rigid << temp_left, temp_right;
} // Kinematics::update

void Kinematics::computeConstrainedToeJacobian(Eigen::VectorXd &q, Eigen::MatrixXd &Jl, Eigen::MatrixXd &Jr) {
    assert_size_vector(q, 22);
    assert_size_matrix(Jl, 3,7);
    assert_size_matrix(Jr, 3,7);

    Kinematics::Cache *ca = &this->cache;

    SymFunction::J_leftToe(ca->J_positionLeft, q);
    SymFunction::J_rightToe(ca->J_positionRight, q);
    SymFunction::J_achilles_constraint(ca->J_achilles, q);

    MatrixXd Jlf_active(3,7), Jrf_active(3,7), Jach_active_lf(1,7), Jach_active_rf(1,7);

    Jlf_active << ca->J_positionLeft.block(0, CassieStateEnum::LeftHipRoll, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftHipYaw, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftHipPitch, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftKneePitch, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftShinPitch, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftHeelSpring, 3, 1),
                  ca->J_positionLeft.block(0, CassieStateEnum::LeftFootPitch, 3, 1);
    Jrf_active << ca->J_positionRight.block(0, CassieStateEnum::RightHipRoll, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightHipYaw, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightHipPitch, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightKneePitch, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightShinPitch, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightHeelSpring, 3, 1),
                  ca->J_positionRight.block(0, CassieStateEnum::RightFootPitch, 3, 1);

    Jach_active_lf << ca->J_achilles.block(0, CassieStateEnum::LeftHipRoll, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipYaw, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftKneePitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftShinPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHeelSpring, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftFootPitch, 1, 1);
    Jach_active_rf << ca->J_achilles.block(1, CassieStateEnum::RightHipRoll, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipYaw, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightKneePitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightShinPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHeelSpring, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightFootPitch, 1, 1);

    Jl = Jlf_active - ca->J_positionLeft.block(0, CassieStateEnum::LeftTarsusPitch, 3, 1) / ca->J_achilles(0, CassieStateEnum::LeftTarsusPitch) * Jach_active_lf;
    Jr = Jrf_active - ca->J_positionRight.block(0, CassieStateEnum::RightTarsusPitch, 3, 1) / ca->J_achilles(1, CassieStateEnum::RightTarsusPitch) * Jach_active_rf;
} // Kinematics::computeConstrainedToeJacobian

void Kinematics::computeConstrainedFootJacobian(Eigen::VectorXd &q, Eigen::MatrixXd &Jl, Eigen::MatrixXd &Jr) {
    assert_size_vector(q, 22);
    assert_size_matrix(Jl, 3,7);
    assert_size_matrix(Jr, 3,7);

    Kinematics::Cache *ca = &this->cache;

    SymFunction::J_leftFoot(ca->J_poseLeft, q);
    SymFunction::J_rightFoot(ca->J_poseRight, q);
    SymFunction::J_achilles_constraint(ca->J_achilles, q);

    MatrixXd Jlf_active(3,7), Jrf_active(3,7), Jach_active_lf(1,7), Jach_active_rf(1,7);

    Jlf_active << ca->J_poseLeft.block(0, CassieStateEnum::LeftHipRoll, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftHipYaw, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftHipPitch, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftKneePitch, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftShinPitch, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftHeelSpring, 3, 1),
                  ca->J_poseLeft.block(0, CassieStateEnum::LeftFootPitch, 3, 1);
    Jrf_active << ca->J_poseRight.block(0, CassieStateEnum::RightHipRoll, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightHipYaw, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightHipPitch, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightKneePitch, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightShinPitch, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightHeelSpring, 3, 1),
                  ca->J_poseRight.block(0, CassieStateEnum::RightFootPitch, 3, 1);

    Jach_active_lf << ca->J_achilles.block(0, CassieStateEnum::LeftHipRoll, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipYaw, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftKneePitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftShinPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHeelSpring, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftFootPitch, 1, 1);
    Jach_active_rf << ca->J_achilles.block(1, CassieStateEnum::RightHipRoll, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipYaw, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightKneePitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightShinPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHeelSpring, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightFootPitch, 1, 1);

    Jl = Jlf_active - ca->J_poseLeft.block(0, CassieStateEnum::LeftTarsusPitch, 3, 1) / ca->J_achilles(0, CassieStateEnum::LeftTarsusPitch) * Jach_active_lf;
    Jr = Jrf_active - ca->J_poseRight.block(0, CassieStateEnum::RightTarsusPitch, 3, 1) / ca->J_achilles(1, CassieStateEnum::RightTarsusPitch) * Jach_active_rf;
} // Kinematics::computeConstrainedFootJacobian

void Kinematics::computeStanceConstrainedJacobian(Eigen::VectorXd &q, Eigen::MatrixXd &Jl, Eigen::MatrixXd &Jr) {
    assert_size_vector(q, 22);
    assert_size_matrix(Jl, 5,7);
    assert_size_matrix(Jr, 5,7);

    Kinematics::Cache *ca = &this->cache;

    // This assumes that kinematics.update() has been called!

    MatrixXd Jlf_active(5,7), Jrf_active(5,7), Jach_active_lf(1,7), Jach_active_rf(1,7);

    Jlf_active << ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHipRoll, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHipYaw, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHipPitch, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftKneePitch, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftShinPitch, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftHeelSpring, 5, 1),
                  ca->J_poseLeftConstraint.block(0, CassieStateEnum::LeftFootPitch, 5, 1);
    Jrf_active << ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHipRoll, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHipYaw, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHipPitch, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightKneePitch, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightShinPitch, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightHeelSpring, 5, 1),
                  ca->J_poseRightConstraint.block(0, CassieStateEnum::RightFootPitch, 5, 1);

    Jach_active_lf << ca->J_achilles.block(0, CassieStateEnum::LeftHipRoll, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipYaw, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHipPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftKneePitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftShinPitch, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftHeelSpring, 1, 1),
                      ca->J_achilles.block(0, CassieStateEnum::LeftFootPitch, 1, 1);
    Jach_active_rf << ca->J_achilles.block(1, CassieStateEnum::RightHipRoll, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipYaw, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHipPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightKneePitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightShinPitch, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightHeelSpring, 1, 1),
                      ca->J_achilles.block(1, CassieStateEnum::RightFootPitch, 1, 1);

    Jl = Jlf_active - ca->J_poseLeft.block(0, CassieStateEnum::LeftTarsusPitch, 5, 1) / ca->J_achilles(0, CassieStateEnum::LeftTarsusPitch) * Jach_active_lf;
    Jr = Jrf_active - ca->J_poseRight.block(0, CassieStateEnum::RightTarsusPitch, 5, 1) / ca->J_achilles(1, CassieStateEnum::RightTarsusPitch) * Jach_active_rf;
} // Kinematics::computeConstrainedFootJacobian

void Dynamics::initialize(Model *model) {
    this->H.resize(model->dof_count,model->dof_count);
    this->C.resize(model->dof_count);
    this->H.setZero();
    this->C.setZero();
} // Dynamics::initialize

void Dynamics::calcHandC(Model *model, VectorXd &Q, VectorXd &QDot) {
    // Evaluate inertia matrix (H) and velocity terms (C)
    VectorXd QDDot(22);
    QDDot.setZero();
    InverseDynamics(*model, Q, QDot, QDDot, this->C);
    CompositeRigidBodyAlgorithm(*model, Q, this->H, false);

    // Add reflected rotor inertias
    this->H(LeftHipRoll,LeftHipRoll)       += 0.038125;
    this->H(RightHipRoll,RightHipRoll)     += 0.038125;
    this->H(LeftHipYaw,LeftHipYaw)         += 0.038125;
    this->H(RightHipYaw,RightHipYaw)       += 0.038125;
    this->H(LeftHipPitch,LeftHipPitch)     += 0.09344;
    this->H(RightHipPitch,RightHipPitch)   += 0.09344;
    this->H(LeftKneePitch,LeftKneePitch)   += 0.09344;
    this->H(RightKneePitch,RightKneePitch) += 0.09344;
    this->H(LeftFootPitch,LeftFootPitch)   += 0.01225;
    this->H(RightFootPitch,RightFootPitch) += 0.01225;
} // Dynamics::calcHandC

Cassie::Cassie(bool verbose) {
    // Retreive the path to the robot URDF description
    std::string urdf_path = ros::package::getPath("cassie_description");
    urdf_path.append("/urdf/cassie_v4.urdf");
    const char* urdf_path_cstr = urdf_path.c_str();

    // Construct the RBDL model
    this->model = new RigidBodyDynamics::Model();
    Addons::URDFReadFromFile(urdf_path_cstr, this->model, true, verbose);

    // Add the leg loop constraints
    // TODO: This is not fully integrated.
    this->constraints.AddLoopConstraint(
        this->model->GetBodyId("lefthippitch"),
        this->model->GetBodyId("leftheelspring"),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0., 0., 0.045)),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0.11877, -0.01, 0.)),
        Math::SpatialVector(0.,0.,0.,1.,1.,1.),
        false,
        0.1,
        "leftachilles");
    this->constraints.AddLoopConstraint(
        this->model->GetBodyId("righthippitch"),
        this->model->GetBodyId("rightheelspring"),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0., 0., -0.045)),
        Math::SpatialTransform(Matrix3d::Identity(3,3), Vector3d(0.11877, -0.01, 0.)),
        Math::SpatialVector(0.,0.,0.,1.,1.,1.),
        false,
        0.1,
        "rightachilles");
    this->constraints.Bind(*this->model);
    this->constraints.clear();

    this->torque.resize(10);
    this->torque.setZero();

    // Create robot statespace
    this->q.resize(22);
    this->dq.resize(22);
    this->ddq.resize(22);

    this->q.setZero();
    this->dq.setZero();
    this->ddq.setZero();
    this->quat_pelvis.setIdentity();

    // Indexing helpers
    this->iRotorMap.resize(10);
    this->iJointMap.resize(4);
    this->iEncoderMap.resize(14);
    this->iRotorMap <<
            CassieStateEnum::LeftHipRoll,
            CassieStateEnum::LeftHipYaw,
            CassieStateEnum::LeftHipPitch,
            CassieStateEnum::LeftKneePitch,
            CassieStateEnum::LeftFootPitch,
            CassieStateEnum::RightHipRoll,
            CassieStateEnum::RightHipYaw,
            CassieStateEnum::RightHipPitch,
            CassieStateEnum::RightKneePitch,
            CassieStateEnum::RightFootPitch;
    this->iJointMap <<
            CassieStateEnum::LeftShinPitch,
            CassieStateEnum::LeftTarsusPitch,
            CassieStateEnum::RightShinPitch,
            CassieStateEnum::RightTarsusPitch;
    this->iEncoderMap <<
            CassieStateEnum::LeftHipRoll,
            CassieStateEnum::LeftHipYaw,
            CassieStateEnum::LeftHipPitch,
            CassieStateEnum::LeftKneePitch,
            CassieStateEnum::LeftShinPitch,
            CassieStateEnum::LeftTarsusPitch,
            CassieStateEnum::LeftFootPitch,
            CassieStateEnum::RightHipRoll,
            CassieStateEnum::RightHipYaw,
            CassieStateEnum::RightHipPitch,
            CassieStateEnum::RightKneePitch,
            CassieStateEnum::RightShinPitch,
            CassieStateEnum::RightTarsusPitch,
            CassieStateEnum::RightFootPitch;

    // Append additional modeling considerations
    this->model->gravity = Vector3d(0., 0., -9.80665);

    // Contact modes
    this->leftContact  = 0.0;
    this->rightContact = 0.0;

    this->GRF.resize(6);
    this->GRF = Eigen::VectorXd::Zero(6);

    // Construct the class members
    this->kinematics.initialize(this->model);
    this->dynamics.initialize(this->model);
} // Cassie::Cassie

} // namespace cassie_model


/* Copyright Institue of Sound and Vibration Research - All rights reserved. */

#include <librrl/audio_signal_flow.hpp>
#include <librrl/integrity_checking.hpp>

#include <libvisr/atomic_component.hpp>
#include <libvisr/parameter_input.hpp>
#include <libvisr/parameter_output.hpp>
#include <libvisr/composite_component.hpp>
#include <libvisr/signal_flow_context.hpp>

#include <libpml/double_buffering_protocol.hpp>
#include <libpml/matrix_parameter.hpp>
#include <libpml/matrix_parameter_config.hpp>
#include <libpml/initialise_parameter_library.hpp>

#include <boost/test/unit_test.hpp>

#include <ciso646>
#include <cstddef>
#include <iostream>
#include <memory>
#include <string>
#include <sstream>
#include <vector>

namespace visr
{
namespace rrl
{
namespace test
{

namespace // unnamed
{

std::size_t const numRows =3;
std::size_t const numCols = 5;

class Sender: public AtomicComponent
{
public:
  Sender( SignalFlowContext & context, char const * componentName, CompositeComponent * parent )
   : AtomicComponent( context, componentName, parent )
   , mParamOutput( "paramOut", *this, pml::MatrixParameterConfig(numRows, numCols) )
  {
  }
  void process() override {}
private:
  ParameterOutput<pml::DoubleBufferingProtocol, pml::MatrixParameter<float> > mParamOutput;
};

class Receiver: public AtomicComponent
{
public:
  Receiver( SignalFlowContext & context, char const * componentName, CompositeComponent * parent )
    : AtomicComponent( context, componentName, parent )
    , mParamInput( "paramIn", *this, pml::MatrixParameterConfig( numRows, numCols ) )
  {
  }
  void process() override
  {
    pml::MatrixParameter<float> const & mtx = mParamInput.data();
    if( (mtx.numberOfRows() != numRows) or (mtx.numberOfColumns() != numCols) )
    {
      status( StatusMessage::Warning, "ParameterConnection::Receiver: Matrix dimension mismatch. Expected (", numRows, ",", numCols,
        ") received (", mtx.numberOfRows(), ",", mtx.numberOfColumns(), ")." );
    }

  }
private:
  ParameterInput<pml::DoubleBufferingProtocol, pml::MatrixParameter<float> > mParamInput;
};

class MyComposite: public CompositeComponent
{
public:
  MyComposite( SignalFlowContext & context, char const * componentName, CompositeComponent * parent )
    : CompositeComponent( context, componentName, parent )
    , mSender( context, "Sender", this )
    , mReceiver( context, "Receiver", this)
  {
    parameterConnection( mSender.parameterPort("paramOut"), mReceiver.parameterPort("paramIn") );
  }
private:
  Sender mSender;
  Receiver mReceiver;
};

} // namespace unnamed

BOOST_AUTO_TEST_CASE( CheckParamConnection )
{
  pml::initialiseParameterLibrary();

  const std::size_t blockSize{64};
  std::size_t const numBlocks{16};

  SignalFlowContext context( blockSize, 48000 );

  MyComposite myComp( context, "topLevel", nullptr );

  std::stringstream checkMsg;
  bool const res = rrl::checkConnectionIntegrity( myComp, true, checkMsg );
  BOOST_CHECK_MESSAGE( res, checkMsg.str().c_str() );

  rrl::AudioSignalFlow flow( myComp );

  float ** inputPtr =  nullptr;
  float ** outputPtr = nullptr;

  for( std::size_t blockIdx(0); blockIdx < numBlocks; ++blockIdx )
  {
    flow.process( inputPtr, outputPtr );
  }

}

} // namespace test
} // namespace rrl
} // namespace visr


/**
 * Non-metric Space Library
 *
 * Authors: Bilegsaikhan Naidan (https://github.com/bileg), Leonid Boytsov (http://boytsov.info).
 * With contributions from Lawrence Cayton (http://lcayton.com/) and others.
 *
 * For the complete list of contributors and further details see:
 * https://github.com/searchivarius/NonMetricSpaceLib
 *
 * Copyright (c) 2015
 *
 * This code is released under the
 * Apache License Version 2.0 http://www.apache.org/licenses/.
 *
 */

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>

#include <cmath>
#include <memory>
#include <limits>
#include <string>
#include <sstream>
#include <vector>
#include <fstream>
#include <map>

#include <boost/program_options.hpp>
#include <cluster_util.h>

#include "init.h"
#include "global.h"
#include "utils.h"
#include "ztimer.h"
#include "experiments.h"
#include "tune.h"
#include "method/vptree.h"
#include "method/proj_vptree.h"
#include "method/perm_bin_vptree.h"
#include "logging.h"
#include "spacefactory.h"
#include "cluster_util.h"
#include "params_def.h"

#include "meta_analysis.h"
#include "params.h"

using namespace similarity;

#define CLUST_TYPE_PARAM_OPT "clustType,t"
const string CLUST_TYPE_PARAM_MSG="A type of cluster: " + CLUST_TYPE_CLARAN + "," + CLUST_TYPE_FIRMAL;
#define CLUST_QTY_PARAM_OPT "clustQty,c"
#define CLUST_QTY_PARAM_MSG "A # of clusters"
const IdTypeUnsign CLUST_QTY_PARAM_DEFAULT = 100;
#define IN_CLUST_SWAP_ATT_PARAM_OPT "swapAtt,W"
#define IN_CLUST_SWAP_ATT_PARAM_MSG "The number of in claster swap attempts (in order to find a better center)"
const IdTypeUnsign IN_CLUST_SWAP_ATT_PARAM_DEFAULT = CLARANS_SWAP_ATTEMPTS;
#define IN_CLUST_SAMPLE_QTY_PARAM_OPT "clustSampleQty,Q"
#define IN_CLUST_SAMPLE_QTY_PARAM_MSG "The number of sampled points inside the cluster to compute a cluster configuration cost"
const IdTypeUnsign IN_CLUST_SAMPLE_QTY_PARAM_DEFAULT = CLARANS_SAMPLE_QTY;
#define RAND_REST_QTY_PARAM_OPT "randRestartQty,R"
#define RAND_REST_QTY_PARAM_MSG "The number of random restarts"
const size_t RAND_REST_QTY_PARAM_DEFAULT = 5;
#define SEARCH_CLOSE_ITER_QTY_PARAM_OPT "searchCloseIterQty,I"
#define SEARCH_CLOSE_ITER_QTY_PARAM_MSG "A number of search iterations to find a point that is close to already selected centers"
const IdTypeUnsign SEARCH_CLOSE_ITER_QTY_PARAM_DEFAULT = 200;
#define DIST_SAMPLE_QTY_PARAM_OPT "distSampleQty,S"
#define DIST_SAMPLE_QTY_PARAM_MSG "A number of samples to determine the distribution of distances"
const IdTypeUnsign DIST_SAMPLE_QTY_PARAM_DEFAULT = SAMPLE_LIST_CLUST_DEFAULT_SAMPLE_QTY;
#define MAX_META_ITER_QTY_PARAM_OPT "maxMetaIterQty,M"
#define MAX_META_ITER_QTY_PARAM_MSG "A maximum number of meta iterations"
const IdTypeUnsign MAX_META_ITER_QTY_PARAM_DEFAULT = 10;
#define KEEP_FRAC_QTY_PARAM_OPT "keepFrac,F"
#define KEEP_FRAC_QTY_PARAM_MSG "Percentage of assigned points kept after a meta-iteration is finished"
const float KEEP_FRAC_QTY_PARAM_DEFAULT = 0.2;

using std::vector;
using std::map;
using std::make_pair;
using std::string;
using std::stringstream;

namespace po = boost::program_options;

static void Usage(const char *prog,
                  const po::options_description& desc) {
    std::cout << prog << std::endl
              << desc << std::endl;
}

template <typename dist_t>
void RunExper(
    bool                   PrintProgress,
    const string&          DistType,
    const string&          SpaceType,
    shared_ptr<AnyParams>  SpaceParams,
    const string&          DataFile,
    IdTypeUnsign           MaxNumData,
    const string&          ClustType,
    IdTypeUnsign           ClustQty,
    // reductive CLARANS parameters
    IdTypeUnsign           maxMetaIterQty,
    float                  keepFrac,
    // CLARANS & reductive CLARANS parameters
    IdTypeUnsign           inClusterSwapAttempts,
    IdTypeUnsign           inClusterSampleQty, // Number of random points to estimate if the swap was beneficial
    size_t                 randRestQty,
    // FIRMAL parameters
    size_t                 searchCloseIterQty, // A # of search iterations to find a point that is close to already selected centers
                                          // For good performance it should be in the order of sqrt(data.size())
    size_t                 sampleDistQty      // A # of sample to determine the distribution of distances)
) {

  unique_ptr<Space<dist_t>> space (SpaceFactoryRegistry<dist_t>::
                                   Instance().CreateSpace(SpaceType, *SpaceParams));

  ObjectVector              data;
  vector<string>            tmp;

  unique_ptr<DataFileInputState> inpState(space->ReadDataset(data, tmp, DataFile, MaxNumData));
  space->UpdateParamsFromFile(*inpState);

  LOG(LIB_INFO) << "Read: " << data.size() << " entries.";

  ObjectVector                                      vCenters;
  ObjectVector                                      vUnassigned;
  vector<shared_ptr<DistObjectPairVector<dist_t>>>  vClusterAssign;

  if (ClustType == CLUST_TYPE_CLARAN) {
    ClusterUtils<dist_t>::doCLARANS(PrintProgress, *space, data, ClustQty, vCenters, vClusterAssign,
                                    inClusterSwapAttempts, inClusterSampleQty, randRestQty);
  } else if (ClustType == CLUST_TYPE_REDUCT_CLARAN) {
    ClusterUtils<dist_t>::doReductiveCLARANS(PrintProgress, *space, data,
                                             maxMetaIterQty, keepFrac,
                                             ClustQty, vCenters, vClusterAssign, vUnassigned,
                                             inClusterSwapAttempts, inClusterSampleQty);
  } else if (ClustType == CLUST_TYPE_FIRMAL) {
    ClusterUtils<dist_t>::doFIRMAL(PrintProgress, *space, data, ClustQty, vCenters, vClusterAssign, vUnassigned,
                                   searchCloseIterQty, sampleDistQty);
  } else {
    throw runtime_error("Unsupported clustering type: " + ClustType);
  }

  LOG(LIB_INFO) << "The number of unassigned points: " << vUnassigned.size();

  ClusterUtils<dist_t>::printAndVerifyClusterStat(*space, vCenters, vClusterAssign, 1000);
}

void ParseCommandLineForClustering(int argc, char*argv[],
   bool&                   PrintProgress,
   string&                 LogFile,
   string&                 DistType,
   string&                 SpaceType,
   shared_ptr<AnyParams>&  SpaceParams,
   string&                 DataFile,
   unsigned&               MaxNumData,
   string&                 ClustType,
   IdTypeUnsign&           ClustQty,
// reductive CLARANS parameters
   IdTypeUnsign&           maxMetaIterQty,
   float&                  keepFrac,
// CLARANS parameters
   IdTypeUnsign&           inClusterSwapAttempts,
   IdTypeUnsign&           inClusterSampleQty, // Number of random points to estimate if the swap was beneficial
   IdTypeUnsign&           RandRestQty,
// FIRMAL parameters
   IdTypeUnsign&           SearchCloseIterQty,// A # of search iterations to find a point that is close to already selected centers
// For good performance it should be in the order of sqrt(data.size())
   IdTypeUnsign&           SampleDistQty      // A # of sample to determine the distribution of distances)

) {
  bool NoPrintProgress;

  po::options_description ProgOptDesc("Allowed options");
  ProgOptDesc.add_options()
      (HELP_PARAM_OPT,          HELP_PARAM_MSG)
      (NO_PROGRESS_PARAM_OPT,   po::bool_switch(&NoPrintProgress), NO_PROGRESS_PARAM_MSG)
      (SPACE_TYPE_PARAM_OPT,    po::value<string>(&SpaceType)->required(),                    SPACE_TYPE_PARAM_MSG)
      (DIST_TYPE_PARAM_OPT,     po::value<string>(&DistType)->default_value(DIST_TYPE_FLOAT), DIST_TYPE_PARAM_MSG)
      (DATA_FILE_PARAM_OPT,     po::value<string>(&DataFile)->required(),                     DATA_FILE_PARAM_MSG)
      (MAX_NUM_DATA_PARAM_OPT,  po::value<unsigned>(&MaxNumData)->default_value(MAX_NUM_DATA_PARAM_DEFAULT), MAX_NUM_DATA_PARAM_MSG)
      (LOG_FILE_PARAM_OPT,      po::value<string>(&LogFile)->default_value(LOG_FILE_PARAM_DEFAULT),          LOG_FILE_PARAM_MSG)
      (CLUST_TYPE_PARAM_OPT,    po::value<string>(&ClustType)->required(),      CLUST_TYPE_PARAM_MSG.c_str())
      (CLUST_QTY_PARAM_OPT,    po::value<IdTypeUnsign>(&ClustQty)->default_value(CLUST_QTY_PARAM_DEFAULT), CLUST_QTY_PARAM_MSG)
      (IN_CLUST_SWAP_ATT_PARAM_OPT, po::value<IdTypeUnsign>(&inClusterSwapAttempts)->default_value(IN_CLUST_SWAP_ATT_PARAM_DEFAULT), IN_CLUST_SWAP_ATT_PARAM_MSG)
      (IN_CLUST_SAMPLE_QTY_PARAM_OPT, po::value<IdTypeUnsign>(&inClusterSampleQty)->default_value(IN_CLUST_SAMPLE_QTY_PARAM_DEFAULT), IN_CLUST_SAMPLE_QTY_PARAM_MSG)
      (RAND_REST_QTY_PARAM_OPT, po::value<IdTypeUnsign>(&RandRestQty)->default_value(RAND_REST_QTY_PARAM_DEFAULT), RAND_REST_QTY_PARAM_MSG)
      (SEARCH_CLOSE_ITER_QTY_PARAM_OPT,po::value<IdTypeUnsign>(&SearchCloseIterQty)->default_value(SEARCH_CLOSE_ITER_QTY_PARAM_DEFAULT), SEARCH_CLOSE_ITER_QTY_PARAM_MSG)
      (DIST_SAMPLE_QTY_PARAM_OPT, po::value<IdTypeUnsign>(&SampleDistQty)->default_value(DIST_SAMPLE_QTY_PARAM_DEFAULT), DIST_SAMPLE_QTY_PARAM_MSG)
      (MAX_META_ITER_QTY_PARAM_OPT, po::value<IdTypeUnsign>(&maxMetaIterQty)->default_value(MAX_META_ITER_QTY_PARAM_DEFAULT), MAX_META_ITER_QTY_PARAM_MSG)
      (KEEP_FRAC_QTY_PARAM_OPT, po::value<float>(&keepFrac)->default_value(KEEP_FRAC_QTY_PARAM_DEFAULT), KEEP_FRAC_QTY_PARAM_MSG)
      ;

  PrintProgress = !NoPrintProgress;

  po::variables_map vm;
  try {
    po::store(po::parse_command_line(argc, argv, ProgOptDesc), vm);
    po::notify(vm);
  } catch (const exception& e) {
    Usage(argv[0], ProgOptDesc);
    LOG(LIB_FATAL) << e.what();
  }

  if (vm.count("help")  ) {
    Usage(argv[0], ProgOptDesc);
    exit(0);
  }

  ToLower(ClustType);
  ToLower(SpaceType);

  try {
    {
      vector<string> SpaceDesc;
      string str = SpaceType;
      ParseSpaceArg(str, SpaceType, SpaceDesc);
      SpaceParams = shared_ptr<AnyParams>(new AnyParams(SpaceDesc));
    }

    if (DataFile.empty()) {
      LOG(LIB_FATAL) << "data file is not specified!";
    }

    if (!DoesFileExist(DataFile)) {
      LOG(LIB_FATAL) << "data file " << DataFile << " doesn't exist";
    }
    CHECK_MSG(MaxNumData < MAX_DATASET_QTY, "The maximum number of points should not exceed" + ConvertToString(MAX_DATASET_QTY));
  } catch (const exception& e) {
    LOG(LIB_FATAL) << "Exception: " << e.what();
  }
}

int main(int argc, char* argv[]) {
  WallClockTimer timer;
  timer.reset();

  bool                    PrintProgress;
  string                  LogFile;
  string                  DistType;
  string                  SpaceType;
  shared_ptr<AnyParams>   SpaceParams;
  string                  ClustType;
  IdTypeUnsign            ClustQty;
  string                  DataFile;
  IdTypeUnsign            MaxNumData;

// reductive CLARANS parameters
  IdTypeUnsign            maxMetaIterQty;
  float                   keepFrac;
  // CLARANS parameters
  IdTypeUnsign           inClusterSwapAttempts;
  IdTypeUnsign           inClusterSampleQty; // Number of random points to estimate if the swap was beneficial
  IdTypeUnsign           RandRestQty;
  // FIRMAL parameters
  IdTypeUnsign           SearchCloseIterQty;// A # of search iterations to find a point that is close to already selected centers
  // For good performance it should be in the order of sqrt(data.size())
  IdTypeUnsign           SampleDistQty;      // A # of sample to determine the distribution of distances)

  ParseCommandLineForClustering(argc, argv,
                                PrintProgress,
                                LogFile,
                                DistType,
                                SpaceType,
                                SpaceParams,
                                DataFile,
                                MaxNumData,
                                ClustType,
                                ClustQty,
      // Reductive CLARANS parameters
                                maxMetaIterQty,
                                keepFrac,
      // CLARANS parameters
                                inClusterSwapAttempts,
                                inClusterSampleQty,
                                RandRestQty,
      // FIRMAL paramaters
                                SearchCloseIterQty,
                                SampleDistQty
  );

  initLibrary(LogFile.empty() ? LIB_LOGSTDERR:LIB_LOGFILE, LogFile.c_str());

  ToLower(DistType);
  ToLower(ClustType);

  if (!SpaceParams) {
    LOG(LIB_FATAL) << "Failed to initialized space parameters!";
  }

  if (DIST_TYPE_INT == DistType) {
    RunExper<int>(PrintProgress,
                  DistType,
                  SpaceType,
                  SpaceParams,
                  DataFile,
                  MaxNumData,
                  ClustType,
                  ClustQty,
                  maxMetaIterQty,
                  keepFrac,
                  inClusterSwapAttempts,
                  inClusterSampleQty,
                  RandRestQty,
                  SearchCloseIterQty,
                  SampleDistQty
    );
  } else if (DIST_TYPE_FLOAT == DistType) {
    RunExper<float>(PrintProgress,
                    DistType,
                    SpaceType,
                    SpaceParams,
                    DataFile,
                    MaxNumData,
                    ClustType,
                    ClustQty,
                    maxMetaIterQty,
                    keepFrac,
                    inClusterSwapAttempts,
                    inClusterSampleQty,
                    RandRestQty,
                    SearchCloseIterQty,
                    SampleDistQty);
  } else if (DIST_TYPE_DOUBLE == DistType) {
    RunExper<double>(PrintProgress,
                     DistType,
                     SpaceType,
                     SpaceParams,
                     DataFile,
                     MaxNumData,
                     ClustType,
                     ClustQty,
                     maxMetaIterQty,
                     keepFrac,
                     inClusterSwapAttempts,
                     inClusterSampleQty,
                     RandRestQty,
                     SearchCloseIterQty,
                     SampleDistQty);
  } else {
    LOG(LIB_FATAL) << "Unknown distance value type: " << DistType;
  }

  timer.split();
  LOG(LIB_INFO) << "Time elapsed = " << timer.elapsed() / 1e6;
  LOG(LIB_INFO) << "Finished at " << LibGetCurrentTime();

  return 0;
}


#include <boost/icl/split_interval_map.hpp>