#include "hoNDArray_fileio.h"
#include "parameterparser.h"
#include "cuNDArray.h"
#include "hoCuNDArray.h"
#include "hoCuNDArray_utils.h"
#include "cuNDArray_utils.h"
#include "vector_td_utilities.h"
#include "hoCuTvOperator.h"
#include "cuCKOpticalFlowSolver.h"
#include "cuLinearResampleOperator.h"
#include "cuDownsampleOperator.h"
#include "GPUTimer.h"
#include "hoCuCgSolver.h"
#include "hoCuNCGSolver.h"
#include "hoCuEncodingOperatorContainer.h"

#include "CBCT_acquisition.h"
#include "CBCT_binning.h"
#include "hoCuConebeamProjectionOperator.h"
#include "hoCuOFConebeamProjectionOperator.h"
#include "cuCGHSOFSolver.h"

#include "hdf5_utils.h"

#include <string>
#include <sstream>
#include <boost/program_options.hpp>
using namespace Gadgetron;
using namespace std;
namespace po = boost::program_options;

boost::shared_ptr< hoCuNDArray<float> >
perform_registration( boost::shared_ptr< hoCuNDArray<float> > volume, unsigned int phase, float of_alpha, float of_beta, unsigned int num_multires_levels )
{
	std::vector<size_t> volume_dims_3d = *volume->get_dimensions();
	volume_dims_3d.pop_back();
	std::vector<size_t> volume_dims_3d_3 = volume_dims_3d;
	volume_dims_3d_3.push_back(3);

	size_t num_elements_3d = volume_dims_3d[0]* volume_dims_3d[1]* volume_dims_3d[2];
	size_t num_phases = volume->get_size(3);

	boost::shared_ptr< hoCuNDArray<float> > host_result_field( new hoCuNDArray<float> );
	{
		std::vector<size_t> volume_dims_4d = volume_dims_3d;
		volume_dims_4d.push_back(3);
		volume_dims_4d.push_back(volume->get_size(3)-1);
		host_result_field->create( &volume_dims_4d );
	}

	// Upload host data to device
	//

	unsigned int counter = 0;
	hoCuNDArray<float> host_moving( &volume_dims_3d, volume->get_data_ptr()+phase*num_elements_3d );

	for( unsigned int i=0; i<num_phases; i++ ){

		if( i==phase )
			continue;

		hoCuNDArray<float> host_fixed( &volume_dims_3d, volume->get_data_ptr()+i*num_elements_3d );

		cuNDArray<float> fixed_image(&host_fixed);
		cuNDArray<float> moving_image(&host_moving);

		boost::shared_ptr< cuLinearResampleOperator<float,3> > R( new cuLinearResampleOperator<float,3>() );

		// Setup solver
		//
		cuCGHSOFSolver<float,3> OFs;
		//cuCKOpticalFlowSolver<float,3> OFs;
		OFs.set_interpolator( R );
		OFs.set_output_mode( cuCKOpticalFlowSolver<float,3>::OUTPUT_VERBOSE );
		OFs.set_max_num_iterations_per_level( 500 );
		OFs.set_num_multires_levels( num_multires_levels );
		OFs.set_alpha(of_alpha);
		//OFs.set_beta(of_beta);
		//OFs.set_limit(0.01f);

		// Run registration
		//
		std::cout << "Penguin " << std::endl;
		boost::shared_ptr< cuNDArray<float> > result = OFs.solve( &fixed_image, &moving_image );

		std::cout << "Penguin " << std::endl;
		cuNDArray<float> dev_sub;
		dev_sub.create( &volume_dims_3d_3, result->get_data_ptr() );

		hoCuNDArray<float> host_sub( &volume_dims_3d_3, host_result_field->get_data_ptr()+counter*num_elements_3d*3 );
		host_sub = *dev_sub.to_host();

		counter++;
	}

	/*
 {
  std::cout << std::endl << "Writing out registration results for phase " << phase << "." << std::endl;
  char filename[256];
  sprintf(&(filename[0]), "def_moving_%i.real", phase);
  write_nd_array<float>(&host_result_image, (char*)filename);
 }
	 */

	{
		// Permute the displacement field (temporal dimension before 'vector' dimension)
		std::vector<size_t> order;
		order.push_back(0); order.push_back(1); order.push_back(2);
		order.push_back(4); order.push_back(3);
		cuNDArray<float> tmp(host_result_field.get()); // permute is too slow on the host
		*host_result_field = *permute(&tmp, &order);

		/*char filename[256];
    sprintf(&(filename[0]), "displacement_field_%i.real", phase);
    write_nd_array<float>(host_result_field.get(), (char*)filename);*/
	}

	return host_result_field;
}

int main(int argc, char** argv)
{
	string acquisition_filename;
	string tv_recon_filename;
	string binning_filename;
	string outputFile;
	uintd3 imageSize;
	floatd3 voxelSize;
	int device;
	unsigned int downsamples;
	unsigned int iterations;
	float of_alpha,of_beta;
	unsigned int num_reg_downsamples;
	po::options_description desc("Allowed options");

	desc.add_options()
    								("help", "produce help message")
    								("acquisition,a", po::value<string>(&acquisition_filename)->default_value("acquisition.hdf5"), "Acquisition data")
    								("samples,n",po::value<unsigned int>(),"Number of samples per ray")
    								("output,f", po::value<string>(&outputFile)->default_value("reconstruction_REG.hdf5"), "Output filename")
    								("tv_recon,t", po::value<string>(&tv_recon_filename)->default_value("reconstruction.real"), "Input TV recon")
    								("size,s",po::value<uintd3>(&imageSize)->default_value(uintd3(512,512,1)),"Image size in pixels")
    								("binning,b",po::value<string>(&binning_filename)->default_value("binning.hdf5"),"Binning file for 4d reconstruction")
    								("SAG","Use exact SAG correction if present")
    								("voxelSize,v",po::value<floatd3>(&voxelSize)->default_value(floatd3(0.488f,0.488f,1.0f)),"Voxel size in mm")
    								("dimensions,d",po::value<floatd3>(),"Image dimensions in mm. Overwrites voxelSize.")
    								("iterations,i",po::value<unsigned int>(&iterations)->default_value(10),"Number of iterations")
    								("device",po::value<int>(&device)->default_value(0),"Number of the device to use (0 indexed)")
    								("downsample,D",po::value<unsigned int>(&downsamples)->default_value(0),"Downsample projections this factor")
    								("reg_downsample,R",po::value<unsigned int>(&num_reg_downsamples)->default_value(1),"Downsample projections this factor")
    								("alpha",po::value<float>(&of_alpha)->default_value(0.05f),"Alpha for optical Flow")
									("beta",po::value<float>(&of_beta)->default_value(1.0f),"Alpha for optical Flow");

	po::variables_map vm;
	po::store(po::parse_command_line(argc, argv, desc), vm);
	po::notify(vm);

	if (vm.count("help")) {
		cout << desc << "\n";
		return 1;
	}

	std::stringstream command_line_string;
	std::cout << "Command line options:" << std::endl;
	for (po::variables_map::iterator it = vm.begin(); it != vm.end(); ++it){
		boost::any a = it->second.value();
		command_line_string << it->first << ": ";
		if (a.type() == typeid(std::string)) command_line_string << it->second.as<std::string>();
		else if (a.type() == typeid(int)) command_line_string << it->second.as<int>();
		else if (a.type() == typeid(unsigned int)) command_line_string << it->second.as<unsigned int>();
		else if (a.type() == typeid(float)) command_line_string << it->second.as<float>();
		else if (a.type() == typeid(vector_td<float,3>)) command_line_string << it->second.as<vector_td<float,3> >();
		else if (a.type() == typeid(vector_td<int,3>)) command_line_string << it->second.as<vector_td<int,3> >();
		else if (a.type() == typeid(vector_td<unsigned int,3>)) command_line_string << it->second.as<vector_td<unsigned int,3> >();
		else command_line_string << "Unknown type" << std::endl;
		command_line_string << std::endl;
	}
	std::cout << command_line_string.str();

	// Load acquisition data
	//

	boost::shared_ptr<CBCT_acquisition> acquisition( new CBCT_acquisition() );
	{
		GPUTimer timer("Loading projections");
		acquisition->load(acquisition_filename);
	}

	// Downsample projections if requested
	//

	{
		GPUTimer timer("Downsampling projections");
		acquisition->downsample(downsamples);
	}

	// Load binning data
	//

	boost::shared_ptr<CBCT_binning> binning_4d( new CBCT_binning() );
	{
		GPUTimer timer("Loading binning data");
		binning_4d->load(binning_filename);
	}

	// Load intermediate reconstruction
	//

	boost::shared_ptr< hoCuNDArray<float> > tv_recon = boost::make_shared<hoCuNDArray<float>>();
	{
		GPUTimer timer("Loading intermediate reconstruction");
		*tv_recon = *read_nd_array<float>(tv_recon_filename.c_str());
	}

	if( tv_recon->get_number_of_dimensions() != 4 ){
		printf("\nInput volume for registration must be four-dimensional");
		exit(1);
	}

	// Configuring...
	//

	uintd2 ps_dims_in_pixels( acquisition->get_projections()->get_size(0),
			acquisition->get_projections()->get_size(1) );

	floatd2 ps_dims_in_mm( acquisition->get_geometry()->get_FOV()[0],
			acquisition->get_geometry()->get_FOV()[1] );

	float SDD = acquisition->get_geometry()->get_SDD();
	float SAD = acquisition->get_geometry()->get_SAD();
	size_t num_phases = binning_4d->get_number_of_bins();

	//float tv_weight = parms.get_parameter('T')->get_float_value();

	floatd3 imageDimensions;
	if (vm.count("dimensions")){
		imageDimensions = vm["dimensions"].as<floatd3>();
		voxelSize = imageDimensions/imageSize;
	}
	else imageDimensions = voxelSize*imageSize;

	std::vector<size_t> is_dims_3d = to_std_vector((uint64d3)imageSize);

	std::vector<size_t> is_dims_4d = is_dims_3d;
	is_dims_4d.push_back(num_phases);

	// Create mask to zero-out the areas of the images that ar enot fully sampled
	// Do this by a 3D unfiltered backprojection of projections of pure ones.
	// - threshold on a 2% level
	//

	hoCuNDArray<float> image_filter( &is_dims_3d );

	{
		GPUTimer timer("Filtering TV reconstruction");
		hoCuNDArray<float> projections( *acquisition->get_projections() );
		boost::shared_ptr<CBCT_binning> binning_3d( new CBCT_binning );
		hoCuConebeamProjectionOperator E;
		fill( &projections, 1.0f );
		binning_3d->set_as_default_3d_bin(projections.get_size(2));
		E.setup( acquisition, binning_3d, imageDimensions);
		E.set_use_filtered_backprojection(false);
		E.mult_MH( &projections, &image_filter );
		if( !E.get_use_offset_correction() ){
			std::cout << std::endl << "Error: currently offset correction is assumed to guide the TV filtering. Offset correction unavailable" << std::endl;
			exit(1);
		}

		float threshold = 0.98f * 0.5f; // 0.5 is the intensity after offset correction
		clamp( &image_filter, threshold, threshold, 0.0f, 1.0f );

		*tv_recon *= image_filter;
	}

	// Downsample TV recon if specified
	// - for every downsample of OF image space, dowmsample OF projections as well
	//

	boost::shared_ptr<hoCuNDArray<float> > projections( new hoCuNDArray<float>( *acquisition->get_projections() ));

	hoCuNDArray<float> image_3d(&is_dims_3d);
	hoCuNDArray<float> image_4d(&is_dims_4d);

	// Define encoding operator for the reconstruction -- a "plain" CBCT operator
	//

	boost::shared_ptr< hoCuConebeamProjectionOperator > E( new hoCuConebeamProjectionOperator() );
	E->setup( acquisition, imageDimensions);
	E->set_domain_dimensions(&is_dims_3d);
	E->set_codomain_dimensions(acquisition->get_projections()->get_dimensions().get());
	E->offset_correct(projections.get());

	boost::shared_ptr<hoCuNDArray<float> > OF_projections = projections;

	if( num_reg_downsamples > 0 ) {
		GPUTimer timer("Downsampling TV reconstruction (and OF operator projections accordingly)");

		std::vector<size_t> tmp_dims_3d = is_dims_3d;
		for( unsigned int i=0; i<tmp_dims_3d.size(); i++ ){
			for( unsigned int d=0; d<num_reg_downsamples; d++ ){
				if( (tmp_dims_3d[i]%2)==1 )
					throw std::runtime_error("Error: input volume for registration must have even size in all dimensions (at all levels) in order to downsample");
				tmp_dims_3d[i] /= 2;
			}
		}

		cuNDArray<float> tmp_image_in(tv_recon.get());
		cuNDArray<float> tmp_proj_in(OF_projections.get());

		std::vector<size_t> volume_dims_4d = *tv_recon->get_dimensions();
		std::vector<size_t> proj_dims_3d = *projections->get_dimensions();

		for( unsigned int d=0; d<num_reg_downsamples; d++ ){

			for( unsigned int i=0; i<3; i++ ) volume_dims_4d[i] /= 2; // do not downsample temporal dimension
			for( unsigned int i=0; i<2; i++ ) proj_dims_3d[i] /= 2;   // do not downsample #projections dimension

			cuNDArray<float> tmp_image_out(&volume_dims_4d);
			cuNDArray<float> tmp_proj_out(&proj_dims_3d);

			cuDownsampleOperator<float,3> D_image;
			cuDownsampleOperator<float,2> D_proj;

			D_image.mult_M( &tmp_image_in, &tmp_image_out );
			D_proj.mult_M( &tmp_proj_in, &tmp_proj_out );

			tmp_image_in = tmp_image_out;
			tmp_proj_in = tmp_proj_out;
		}

		*tv_recon = tmp_image_in;
		OF_projections = boost::shared_ptr<hoCuNDArray<float> >( new hoCuNDArray<float>( *tmp_proj_in.to_host() ));
	}

	// Allocate 3d array for phase-by-phase reconstruction
	// and 4D array to hold the overall result
	//

	// Define the optical flow regularization operator
	//

	boost::shared_ptr< hoCuOFConebeamProjectionOperator > OF( new hoCuOFConebeamProjectionOperator() );
	OF->setup( acquisition, binning_4d, imageDimensions);
	OF->set_domain_dimensions(&is_dims_3d);
	OF->set_codomain_dimensions(OF_projections->get_dimensions().get());

	//OF->set_weight(1.0f/float(num_phases));

	// Combine the two operators in an operator container
	//

	boost::shared_ptr< hoCuEncodingOperatorContainer<float> > opContainer( new hoCuEncodingOperatorContainer<float>() );

	opContainer->add_operator(E);
	opContainer->add_operator(OF);

	std::vector<hoCuNDArray<float>*> codoms;
	codoms.push_back( projections.get() );
	codoms.push_back( OF_projections.get() );

	boost::shared_ptr< hoCuNDArray<float> > f = opContainer->create_codomain( codoms );
	codoms.clear(); projections.reset(); OF_projections.reset();

	// Setup solver
	//

	hoCuCgSolver<float> solver;
	//hoCuNCGSolver<float> solver;

	solver.set_encoding_operator( opContainer );

	/*if( tv_weight > 0.0f ){
		boost::shared_ptr<hoCuTvOperator<float,3> > tvOp(new hoCuTvOperator<float,3>());
		tvOp->set_weight(tv_weight);
		tvOp->set_limit(float(1e-6));
		solver.add_nonlinear_operator(tvOp);
    }*/

	//solver.set_non_negativity_constraint(true);
	//solver.set_output_mode(hoCuNCGSolver<float>::OUTPUT_VERBOSE);
	solver.set_output_mode(hoCuCgSolver<float>::OUTPUT_VERBOSE);
	solver.set_max_iterations(iterations);
	solver.set_tc_tolerance(float(1e-9));

	// Solve
	//

	//for( unsigned int phase=0; phase<binning_4d->get_number_of_bins(); phase++ ){

	for( unsigned int phase=0; phase<num_phases; phase++ ){

		// Define the binning for the current phase
		//

		std::cout << "Phase " << phase << std::endl;
		std::vector<unsigned int> bin = binning_4d->get_bin(phase);
		boost::shared_ptr<CBCT_binning> binning_phase( new CBCT_binning() );
		binning_phase->set_bin( bin, 0 );
		E->set_binning(binning_phase);

		OF->set_encoding_phase(phase);
		{
			boost::shared_ptr<hoCuNDArray<float>> displacements;
			{
			GPUTimer timer("Registration");
			displacements =	perform_registration( tv_recon, phase, of_alpha, of_beta, 3 - num_reg_downsamples );
			}
			{
			GPUTimer timer("Setting displacements");
			OF->set_displacement_field(displacements);
			}
		}

		boost::shared_ptr<hoCuNDArray<float> > result_phase = solver.solve( f.get() );

		// Copy result to 4d array
		//

		hoCuNDArray<float> host_result_3d( &is_dims_3d, image_4d.get_data_ptr()+phase*result_phase->get_number_of_elements() );
		host_result_3d = *result_phase;

		// Apply "cropping" filter
		//

		host_result_3d *= image_filter;

		// Write out every 3d volume
		//

		/*{
			char filename[256];
			sprintf(&(filename[0]), "reconstruction_3d_%i.real", phase);
			write_nd_array<float>( result_phase.get(), (char*)filename );
			}*/
	}

	saveNDArray2HDF5(&image_4d,outputFile,imageDimensions,floatd3(0,0,0),command_line_string.str(),iterations);

	return 0;
}


#include "graph.h"

#include <boost/test/unit_test.hpp>

#include <algorithm>
#include <iostream>

using namespace anyf;

namespace {

int identity(int x) { return x; }
int cidentity(const int& x) { return x; }
int by2(const int& x) { return x * 2; }
auto sum(int x, int y) { return x + y; }

bool compare_edges_to_nodes(const std::vector<Edge>& expected, const Node& actual) {
  return std::equal(expected.begin(), expected.end(), actual.outputs.begin(), actual.outputs.end());
}

bool compare_node_outputs(const Node& expected, const Node& actual) { return expected.outputs == actual.outputs; }

} // namespace

BOOST_AUTO_TEST_CASE(simple_graph_construction) {
  auto [cg, inputs] = make_graph(make_types(TL<int, int>{}));
  const auto id_outputs = cg.add(AnyFunction(cidentity), std::array{inputs[0]});
  const auto sum_outputs = cg.add(AnyFunction(sum), std::array{id_outputs[0], inputs[1]});
  const FunctionGraph g = finalize(std::move(cg), sum_outputs);

  const std::vector<std::vector<Edge>> expected_edges{{{0, {1, 0}}, {1, {2, 1}}}, {{0, {2, 0}}}, {{0, {3, 0}}}, {}};

  BOOST_CHECK((std::vector<Term>{{0, 0}, {0, 1}}) == inputs);
  BOOST_CHECK((std::vector<Term>{{1, 0}}) == id_outputs);
  BOOST_CHECK((std::vector<Term>{{2, 0}}) == sum_outputs);

  BOOST_CHECK(std::equal(expected_edges.begin(), expected_edges.end(), g.begin(), g.end(), compare_edges_to_nodes));

  BOOST_CHECK(make_types(TL<int, int>{}) == std::get<std::vector<Type>>(g.front().func));
  BOOST_CHECK(make_types(TL<int>{}) == std::get<std::vector<Type>>(g.back().func));
}

BOOST_AUTO_TEST_CASE(simple_inner_graph) {

  auto [inner_cg, inner_inputs] = make_graph(make_types(TL<int, int>{}));
  const auto inner_id_outputs = inner_cg.add(AnyFunction(cidentity), std::array{inner_inputs[0]});
  const auto inner_sum_outputs = inner_cg.add(AnyFunction(sum), std::array{inner_id_outputs[0], inner_inputs[1]});
  const FunctionGraph inner_g = finalize(std::move(inner_cg), inner_sum_outputs);

  auto [cg, inputs] = make_graph(make_types(TL<int, int>{}));
  const auto id_outputs = cg.add(AnyFunction(identity), std::array{inputs[0]});
  const auto g_outputs = cg.add(inner_g, std::array{id_outputs[0], inputs[1]});
  const auto by2_outputs = cg.add(AnyFunction(by2), g_outputs);
  const FunctionGraph g = finalize(std::move(cg), by2_outputs);

  BOOST_CHECK((std::vector<Term>{{0, 0}, {0, 1}}) == inputs);
  BOOST_CHECK((std::vector<Term>{{1, 0}}) == id_outputs);
  BOOST_CHECK((std::vector<Term>{{3, 0}}) == g_outputs);
  BOOST_CHECK((std::vector<Term>{{4, 0}}) == by2_outputs);

  const std::vector<std::vector<Edge>> expected_edges{
    {{0, {1, 0}}, {1, {3, 1}}}, {{0, {2, 0}}}, {{0, {3, 0}}}, {{0, {4, 0}}}, {{0, {5, 0}}}, {}};

  BOOST_CHECK(std::equal(expected_edges.begin(), expected_edges.end(), g.begin(), g.end(), compare_edges_to_nodes));
}


#ifndef _LDAPLUSPLUS_PARAMETERS_HPP_
#define _LDAPLUSPLUS_PARAMETERS_HPP_

#include <utility>

#include <Eigen/Core>

namespace ldaplusplus {
namespace parameters {

/**
 * All the parameter related objects will be extending this empty struct.
 */
struct Parameters
{
};

/**
 * ModelParameters contain the basic LDA model parameters namely the prior for
 * the documents over topics distribution and the topics over words
 * distributions.
 */
template <typename Scalar = double>
struct ModelParameters : public Parameters
{
    ModelParameters() {}
    ModelParameters(
        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> a,
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> b
    ) : alpha(std::move(a)),
        beta(std::move(b))
    {}

    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> alpha;
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> beta;
};

/**
 * SupervisedModelParameters adds the extra logistic regression parameters to
 * model parameters.
 */
template <typename Scalar = double>
struct SupervisedModelParameters : public ModelParameters<Scalar>
{
    SupervisedModelParameters() {}
    SupervisedModelParameters(
        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> a,
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> b,
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> e
    ) : ModelParameters<Scalar>(a, b),
        eta(std::move(e))
    {}

    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> eta;
};

/**
 * The variational parameters are (duh) the variational parameters of the LDA
 * model.
 */
template <typename Scalar = double>
struct VariationalParameters : public Parameters
{
    VariationalParameters() {}
    VariationalParameters(
        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> g,
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> p
    ) : gamma(std::move(g)),
        phi(std::move(p))
    {}

    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> gamma;
    Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> phi;
};

/**
 * The supervised correspondence variational parameters add a variational
 * parameter for sampling a topic assignment to predict the class.
 */
template <typename Scalar = double>
struct SupervisedCorrespondenceVariationalParameters : public VariationalParameters<Scalar>
{
    SupervisedCorrespondenceVariationalParameters() {}
    SupervisedCorrespondenceVariationalParameters(
        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> g,
        Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> p,
        Eigen::Matrix<Scalar, Eigen::Dynamic, 1> t
    ) : VariationalParameters<Scalar>(g, p),
        tau(std::move(t))
    {}

    Eigen::Matrix<Scalar, Eigen::Dynamic, 1> tau;
};

}  // namespace parameters
}  // namespace ldaplusplus

#endif  // _LDAPLUSPLUS_PARAMETERS_HPP_


//==============================================================================
//         Copyright 2003 - 2011   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef NT2_TRIGONOMETRIC_CONSTANTS_45_HPP_INCLUDED
#define NT2_TRIGONOMETRIC_CONSTANTS_45_HPP_INCLUDED

#include <nt2/include/functor.hpp>
#include <boost/simd/constant/hierarchy.hpp>
#include <boost/simd/constant/register.hpp>

namespace nt2
{
  namespace tag
  {
   /*!
     @brief _45 generic tag

     Represents the _45 constant in generic contexts.

     @par Models:
        Hierarchy
   **/
    BOOST_SIMD_CONSTANT_REGISTER( _45, double
                                , 45, 0x42340000
                                , 0x4046800000000000ll
                                )
  }
  namespace ext
  {
   template<class Site>
   BOOST_FORCEINLINE generic_dispatcher<tag::_45, Site> dispatching__45(adl_helper, boost::dispatch::meta::unknown_<Site>, ...)
   {
     return generic_dispatcher<tag::_45, Site>();
   }
   template<class... Args>
   struct impl__45;
  }
  /*!
    Constant 45.

    @par Semantic:

    For type T0:

    @code
    T0 r = _45<T0>();
    @endcode

    is similar to:

    @code
    T0 r = 45;
    @endcode

    @return a value of type T0
  **/
  BOOST_SIMD_CONSTANT_IMPLEMENTATION(tag::_45, _45);
}

#endif


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2013, 2014, 2015.
// Modifications copyright (c) 2013-2015 Oracle and/or its affiliates.

// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

// Use, modification and distribution is 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_GEOMETRY_ALGORITHMS_DETAIL_RELATE_POINT_GEOMETRY_HPP
#define BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_POINT_GEOMETRY_HPP

#include <boost/geometry/algorithms/detail/within/point_in_geometry.hpp>
//#include <boost/geometry/algorithms/within.hpp>
//#include <boost/geometry/algorithms/covered_by.hpp>

#include <boost/geometry/algorithms/detail/relate/result.hpp>
#include <boost/geometry/algorithms/detail/relate/topology_check.hpp>

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

namespace geofeatures_boost {} namespace boost = geofeatures_boost; namespace geofeatures_boost { namespace geometry
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail { namespace relate {

// non-point geometry
template <typename Point, typename Geometry, bool Transpose = false>
struct point_geometry
{
    // TODO: interrupt only if the topology check is complex

    static const bool interruption_enabled = true;

    template <typename Result>
    static inline void apply(Point const& point, Geometry const& geometry, Result & result)
    {
        int pig = detail::within::point_in_geometry(point, geometry);

        if ( pig > 0 ) // within
        {
            relate::set<interior, interior, '0', Transpose>(result);
        }
        else if ( pig == 0 )
        {
            relate::set<interior, boundary, '0', Transpose>(result);
        }
        else // pig < 0 - not within
        {
            relate::set<interior, exterior, '0', Transpose>(result);
        }

        relate::set<exterior, exterior, result_dimension<Point>::value, Transpose>(result);

        if ( BOOST_GEOMETRY_CONDITION(result.interrupt) )
            return;

        // the point is on the boundary
        if ( pig == 0 )
        {
            // NOTE: even for MLs, if there is at least one boundary point,
            // somewhere there must be another one

            // check if there are other boundaries outside
            typedef detail::relate::topology_check<Geometry> tc_t;
            //tc_t tc(geometry, point);
            //if ( tc.has_interior )
                relate::set<exterior, interior, tc_t::interior, Transpose>(result);
            //if ( tc.has_boundary )
                relate::set<exterior, boundary, tc_t::boundary, Transpose>(result);
        }
        else
        {
            // check if there is a boundary in Geometry
            typedef detail::relate::topology_check<Geometry> tc_t;
            tc_t tc(geometry);
            if ( tc.has_interior )
                relate::set<exterior, interior, tc_t::interior, Transpose>(result);
            if ( tc.has_boundary )
                relate::set<exterior, boundary, tc_t::boundary, Transpose>(result);
        }
    }
};

// transposed result of point_geometry
template <typename Geometry, typename Point>
struct geometry_point
{
    // TODO: interrupt only if the topology check is complex

    static const bool interruption_enabled = true;

    template <typename Result>
    static inline void apply(Geometry const& geometry, Point const& point, Result & result)
    {
        point_geometry<Point, Geometry, true>::apply(point, geometry, result);
    }
};

// TODO: rewrite the folowing:

//// NOTE: Those tests should be consistent with within(Point, Box) and covered_by(Point, Box)
//// There is no EPS used in those functions, values are compared using < or <=
//// so comparing MIN and MAX in the same way should be fine
//
//template <typename Box, std::size_t I = 0, std::size_t D = geometry::dimension<Box>::value>
//struct box_has_interior
//{
//    static inline bool apply(Box const& box)
//    {
//        return geometry::get<min_corner, I>(box) < geometry::get<max_corner, I>(box)
//            && box_has_interior<Box, I + 1, D>::apply(box);
//    }
//};
//
//template <typename Box, std::size_t D>
//struct box_has_interior<Box, D, D>
//{
//    static inline bool apply(Box const&) { return true; }
//};
//
//// NOTE: especially important here (see the NOTE above).
//
//template <typename Box, std::size_t I = 0, std::size_t D = geometry::dimension<Box>::value>
//struct box_has_equal_min_max
//{
//    static inline bool apply(Box const& box)
//    {
//        return geometry::get<min_corner, I>(box) == geometry::get<max_corner, I>(box)
//            && box_has_equal_min_max<Box, I + 1, D>::apply(box);
//    }
//};
//
//template <typename Box, std::size_t D>
//struct box_has_equal_min_max<Box, D, D>
//{
//    static inline bool apply(Box const&) { return true; }
//};
//
//template <typename Point, typename Box>
//struct point_box
//{
//    static inline result apply(Point const& point, Box const& box)
//    {
//        result res;
//
//        if ( geometry::within(point, box) ) // this also means that the box has interior
//        {
//            return result("0FFFFFTTT");
//        }
//        else if ( geometry::covered_by(point, box) ) // point is on the boundary
//        {
//            //if ( box_has_interior<Box>::apply(box) )
//            //{
//            //    return result("F0FFFFTTT");
//            //}
//            //else if ( box_has_equal_min_max<Box>::apply(box) ) // no boundary outside point
//            //{
//            //    return result("F0FFFFFFT");
//            //}
//            //else // no interior outside point
//            //{
//            //    return result("F0FFFFFTT");
//            //}
//            return result("F0FFFF**T");
//        }
//        else
//        {
//            /*if ( box_has_interior<Box>::apply(box) )
//            {
//                return result("FF0FFFTTT");
//            }
//            else
//            {
//                return result("FF0FFFFTT");
//            }*/
//            return result("FF0FFF*TT");
//        }
//
//        return res;
//    }
//};
//
//template <typename Box, typename Point>
//struct box_point
//{
//    static inline result apply(Box const& box, Point const& point)
//    {
//        if ( geometry::within(point, box) )
//            return result("0FTFFTFFT");
//        else if ( geometry::covered_by(point, box) )
//            return result("FF*0F*FFT");
//        else
//            return result("FF*FFT0FT");
//    }
//};

}} // namespace detail::relate
#endif // DOXYGEN_NO_DETAIL

}} // namespace geofeatures_boost::geometry

#endif // BOOST_GEOMETRY_ALGORITHMS_DETAIL_RELATE_POINT_GEOMETRY_HPP


/*
 * calc/network.hpp --
 *
 * This file is part of nettcl2d application.
 *
 * Copyright (c) 2012 Andrey V. Nakin <andrey.nakin@gmail.com>
 * All rights reserved.
 *
 * See the file "COPYING" for information on usage and redistribution
 * of this file, and for a DISCLAIMER OF ALL WARRANTIES.
 *
 */

#ifndef CALC_NETWORK_HPP_
#define CALC_NETWORK_HPP_

#include <vector>
#include <boost/shared_ptr.hpp>
#include <phlib/cloneable.hpp>
#include "contact.hpp"
#include "circuit.hpp"

class Network : public phlib::Cloneable {

	virtual phlib::Cloneable* doClone() const {
		return new Network(*this);
	}

	Network(const Network& src) : contacts(src.contacts), circuits(src.circuits) {}

public:

	typedef std::size_t index_type;
	typedef std::vector<Network::index_type> IndexVector;

	typedef std::vector<Contact> ContactVector;
	typedef ContactVector::iterator contact_iterator;
	typedef ContactVector::const_iterator contact_const_iterator;

	typedef std::vector<Circuit> CircuitVector;
	typedef CircuitVector::iterator circuit_iterator;
	typedef CircuitVector::const_iterator circuit_const_iterator;

	Network() {
	}

	ContactVector::size_type getNumOfContacts() const {
		return contacts.size();
	}

	CircuitVector::size_type getNumOfCircuits() const {
		return circuits.size();
	}

	Contact& contact(std::size_t const index) {
		return contacts[index];
	}

	const Contact& contact(std::size_t const index) const {
		return contacts[index];
	}

	Circuit& circuit(std::size_t const index) {
		return circuits[index];
	}

	const Circuit& circuit(std::size_t const index) const {
		return circuits[index];
	}

	contact_const_iterator contactBegin() const {
		return contacts.begin();
	}

	contact_iterator contactBegin() {
		return contacts.begin();
	}

	contact_const_iterator contactEnd() const {
		return contacts.end();
	}

	contact_iterator contactEnd() {
		return contacts.end();
	}

	circuit_const_iterator circuitBegin() const {
		return circuits.begin();
	}

	circuit_iterator circuitBegin() {
		return circuits.begin();
	}

	circuit_const_iterator circuitEnd() const {
		return circuits.end();
	}

	circuit_iterator circuitEnd() {
		return circuits.end();
	}

	std::size_t addContact(const Contact& c) {
		const std::size_t index = contacts.size();
		contacts.push_back(c);
		return index;
	}

	std::size_t addCircuit(const Circuit& c) {
		const std::size_t index = circuits.size();
		circuits.push_back(c);
		return index;
	}

	IndexVector buildContactIndices(const std::string& expr) const {
		return buildIndices(expr, contactBegin(), contactEnd());
	}

	IndexVector buildCircuitIndices(const std::string& expr) const {
		return buildIndices(expr, circuitBegin(), circuitEnd());
	}

	double flux(const index_type circuitIndex) const {
		const Circuit& c = circuit(circuitIndex);
		double sum = 0.0;

		for (Circuit::const_iterator i = c.begin(), last = c.end(); i != last; ++i) {
			sum += i->weight * contact(i->index).phase;
		}

		return c.square * sum;
	}

private:

	ContactVector contacts;
	CircuitVector circuits;

	template <typename Iterator>
	IndexVector buildIndices(const std::string& expr, Iterator begin, Iterator end) const {
		IndexVector indices;

		for (Iterator i = begin; i != end; ++i) {
			if (expr.empty() || i->matches(expr)) {
				indices.push_back(std::distance(begin, i));
			}
		}

		return indices;
	}

};

#endif /* CALC_NETWORK_HPP_ */


//=======================================================================
// Copyright 2015 - 2020 Jeff Linahan
//
// 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)
//=======================================================================
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/copy.hpp>
#include <iostream>
using namespace std;

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, uint32_t, float> AdjacencyList;

typedef AdjacencyList::vertex_descriptor VertexID;

    struct custom_property {
        //custom_property(uint32_t label = 0, float f = 0) : label(label), f(f) {}
        uint32_t label;
        float f;
    };

typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::undirectedS, custom_property, float> AdjacencyListCustom;

struct vertex_copier {
    AdjacencyList& from;
    AdjacencyListCustom& to;

    void operator()(AdjacencyList::vertex_descriptor input, AdjacencyListCustom::vertex_descriptor output) const {
        to[output] = { from[input], 0.f };
    }
};

int main(int argc, char **argv) {

	cout << "copy test\n";
	AdjacencyList adj;
	VertexID id_0 = boost::add_vertex(0, adj);
	VertexID id_1 = boost::add_vertex(1, adj);
	VertexID id_2 = boost::add_vertex(2, adj);
	VertexID id_3 = boost::add_vertex(4, adj);
	boost::add_edge(id_0, id_1, 1.0f, adj);
	boost::add_edge(id_2, id_3, 2.0f, adj);

	AdjacencyListCustom adj_custom;
	//boost::copy_graph(adj, adj_custom, boost::vertex_copy(vertex_copier{adj, adj_custom}));
}


/*
 * Copyright (c) 2011 Adrian Michel
 * http://www.amichel.com
 *
 * Permission to use, copy, modify, distribute and sell this
 * software and its documentation for any purpose is hereby
 * granted without fee, provided that both the above copyright
 * notice and this permission notice appear in all copies and in
 * the supporting documentation.
 *
 * This library is distributed in the hope that it will be
 * useful. However, Adrian Michel makes no representations about
 * the suitability of this software for any purpose.  It is
 * provided "as is" without any express or implied warranty.
 *
 * Should you find this library useful, please email
 * info@amichel.com with a link or other reference
 * to your work.
 */

#ifndef DE_DIFFERENTIAL_EVOLUTION_HPP_INCLUDED
#define DE_DIFFERENTIAL_EVOLUTION_HPP_INCLUDED

// MS compatible compilers support #pragma once

#if defined(_MSC_VER) && (_MSC_VER >= 1020)
#pragma once
#endif

#include <boost/shared_ptr.hpp>
#include <boost/make_shared.hpp>
#include <boost/enable_shared_from_this.hpp>
#include <boost/shared_array.hpp>
#include <boost/scope_exit.hpp>

#include "random_generator.hpp"
#include "multithread.hpp"
#include "individual.hpp"
#include "processors.hpp"
#include "mutation_strategy.hpp"
#include "population.hpp"
#include "selection_strategy.hpp"
#include "termination_strategy.hpp"
#include "listener.hpp"

namespace de
{

/**
 * Exception thrown in case of an error during an optimization
 * session
 *
 * @author adrian (12/1/2011)
 */
class differential_evolution_exception
{
};

/**
 * Differential evolution main class
 *
 * Runs an optimization session based on various input
 * parameters or strategies
 *
 * @author adrian (12/1/2011)
 */
template< typename T > class differential_evolution
{
private:
    const size_t m_varCount;
    const size_t m_popSize;

    population_ptr m_pop1;
    population_ptr m_pop2;
    individual_ptr m_bestInd;

    constraints_ptr m_constraints;
    typename processors< T >::processors_ptr m_processors;
    termination_strategy_ptr m_terminationStrategy;
    selection_strategy_ptr m_selectionStrategy;
    mutation_strategy_ptr m_mutationStrategy;
    listener_ptr m_listener;

    const bool m_minimize;
public:
    /**
     * constructs a differential_evolution object
     *
     * @author adrian (12/4/2011)
     *
     * @param varCount total number of variables. It includes the
     *  			   variables required by the objective function
     *  			   but has many more elements as required by the
     *  			   algorithm
     * @param popSize total number of individuals in a population
     * @param processors number of parallel processors used
     *  				 during an optimization session
     * @param constraints a vector of constraints that contains the
     *  				  constraints for the variables used by the
     *  				  objective function as well as constraints
     *  				  for all other variables used internally by
     *  				  the algorithm
     * @param minimize will attempt to minimize the cost if true, or
     *  			   maximize the cost if false
     * @param terminationStrategy a termination strategy
     * @param selectionStrategy a selection strategy
     * @param mutationStrategy a mutation strategy
     * @param listener a listener
     */
    differential_evolution( size_t varCount, size_t popSize, typename processors< T >::processors_ptr processors, constraints_ptr constraints, bool minimize,
                            termination_strategy_ptr terminationStrategy, selection_strategy_ptr selectionStrategy,
                            mutation_strategy_ptr mutationStrategy, de::listener_ptr listener )
    try

        : m_varCount( varCount ), m_popSize( popSize ), m_pop1( boost::make_shared< population >( popSize, varCount, constraints ) ),
              m_pop2( boost::make_shared< population >( popSize, varCount ) ), m_bestInd( m_pop1->best( minimize ) ),
              m_constraints( constraints ), m_processors( processors ), m_minimize( minimize ), m_terminationStrategy( terminationStrategy ),
              m_listener( listener ), m_selectionStrategy( selectionStrategy ), m_mutationStrategy( mutationStrategy )
	{
            assert( processors );
            assert( constraints );
            assert( terminationStrategy );
            assert( selectionStrategy );
            assert( listener );
            assert( mutationStrategy );

            assert( popSize > 0 );
            assert( varCount > 0 );

            // initializing population 1 by running all objective functions with
            // the initial random arguments
            processors->push( m_pop1 );
            processors->start();
            processors->wait();

	}
    catch( const processors_exception&)
    {
        throw differential_evolution_exception();
    }

    virtual ~differential_evolution(void)
    {
    }

    /**
     * starts a differential evolution optimization process
     *
     * although the processing is done in parallel, this function is
     * synchronous and won't return until the optimization is
     * complete, or an error triggered an exception
     *
     * @author adrian (12/4/2011)
     */
    void run()
    {
        try
        {
            m_listener->start();
            individual_ptr bestIndIteration( m_bestInd );

            for( size_t genCount = 0 ; m_terminationStrategy->event( m_bestInd, genCount ); ++genCount )
            {
                m_listener->startGeneration( genCount );
                for( size_t i = 0; i < m_popSize; ++i)
                {
                    mutation_strategy::mutation_info mutationInfo( ( *m_mutationStrategy)( *m_pop1, bestIndIteration, i ) );

                    individual_ptr tmpInd( boost::tuples::get< 0 >( mutationInfo ) );

                    tmpInd->ensureConstraints( m_constraints, boost::tuples::get< 1 >( mutationInfo ) );

                    // populate the queue
                    m_processors->push( tmpInd );

                    // put temps in a temp vector for now (they are empty until processed), will be moved to the right place
                    // after processed
                    (*m_pop2)[ i ] = tmpInd;
                }

                m_listener->startProcessors( genCount );
                m_processors->start();
                m_processors->wait();
                m_listener->endProcessors( genCount );

                //BestParentChildSelectionStrategy()( m_pop1, m_pop2, m_bestInd, m_minimize );
                m_listener->startSelection( genCount );
                (*m_selectionStrategy)( m_pop1, m_pop2, m_bestInd, m_minimize );
                bestIndIteration = m_bestInd;

                m_listener->endSelection( genCount );

                m_listener->endGeneration( genCount, bestIndIteration, m_bestInd );

            }

            BOOST_SCOPE_EXIT_TPL( (m_listener) )
            {
                m_listener->end();
            }
            BOOST_SCOPE_EXIT_END
		}
        catch( const processors_exception& )
        {
            m_listener->error();
            throw differential_evolution_exception();
        }

    }

    /**
     * returns the best individual resulted from the optimization
     * process
     *
     * @author adrian (12/4/2011)
     *
     * @return individual_ptr
     */
    individual_ptr best() const { return m_bestInd; }
};

}

#endif //DE_DIFFERENTIAL_EVOLUTION_HPP_INCLUDED


/*
  [auto_generated]
  boost/numeric/odeint/stepper/velocity_verlet.hpp

  [begin_description]
  tba.
  [end_description]

  Copyright 2009-2012 Karsten Ahnert
  Copyright 2009-2012 Mario Mulansky

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE_1_0.txt or
  copy at http://www.boost.org/LICENSE_1_0.txt)
*/

#ifndef BOOST_NUMERIC_ODEINT_STEPPER_VELOCITY_VERLET_HPP_DEFINED
#define BOOST_NUMERIC_ODEINT_STEPPER_VELOCITY_VERLET_HPP_DEFINED

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

#include <boost/numeric/odeint/algebra/algebra_dispatcher.hpp>
#include <boost/numeric/odeint/algebra/operations_dispatcher.hpp>
#include <boost/numeric/odeint/util/resizer.hpp>
#include <boost/numeric/odeint/util/state_wrapper.hpp>
#include <boost/numeric/odeint/util/unwrap_reference.hpp>

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

namespace boost {
namespace numeric {
namespace odeint {

template <
    class Coor ,
    class Velocity = Coor ,
    class Value = double ,
    class Acceleration = Coor ,
    class Time = Value ,
    class TimeSq = Time ,
    class Algebra = typename algebra_dispatcher< Coor >::algebra_type ,
    class Operations = typename operations_dispatcher< Coor >::operations_type ,
    class Resizer = initially_resizer
      >
class velocity_verlet : public algebra_stepper_base< Algebra , Operations >
{
public:

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

    typedef Coor coor_type;
    typedef Velocity velocity_type;
    typedef Acceleration acceleration_type;
    typedef std::pair< coor_type , velocity_type > state_type;
    typedef std::pair< velocity_type , acceleration_type > deriv_type;
    typedef state_wrapper< acceleration_type > wrapped_acceleration_type;
    typedef Value value_type;
    typedef Time time_type;
    typedef TimeSq time_square_type;
    typedef Resizer resizer_type;
    typedef stepper_tag stepper_category;

    typedef unsigned short order_type;

    static const order_type order_value = 1;

    /**
     * \return Returns the order of the stepper.
     */
    order_type order( void ) const
    {
        return order_value;
    }

    velocity_verlet( const algebra_type & algebra = algebra_type() )
        : algebra_stepper_base_type( algebra ) , m_first_call( true )
        , m_a1() , m_a2() , m_current_a1( true ) { }

    template< class System , class StateInOut >
    void do_step( System system , StateInOut & x , time_type t , time_type dt )
    {
        do_step_v1( system , x , t , dt );
    }

    template< class System , class StateInOut >
    void do_step( System system , const StateInOut & x , time_type t , time_type dt )
    {
        do_step_v1( system , x , t , dt );
    }

    template< class System , class CoorIn , class VelocityIn , class AccelerationIn ,
                             class CoorOut , class VelocityOut , class AccelerationOut >
    void do_step( System system , CoorIn const & qin , VelocityIn const & pin , AccelerationIn const & ain ,
                  CoorOut & qout , VelocityOut & pout , AccelerationOut & aout , time_type t , time_type dt )
    {
        const value_type one = static_cast< value_type >( 1.0 );
        const value_type one_half = static_cast< value_type >( 0.5 );

        algebra_stepper_base_type::m_algebra.for_each4(
            qout , qin , pin , ain ,
            typename operations_type::template scale_sum3< value_type , time_type , time_square_type >( one , one * dt , one_half * dt * dt ) );

        typename odeint::unwrap_reference< System >::type & sys = system;

        sys( qout , pin , aout , t + dt );

        algebra_stepper_base_type::m_algebra.for_each4(
            pout , pin , ain , aout ,
            typename operations_type::template scale_sum3< value_type , time_type , time_type >( one , one_half * dt , one_half * dt ) );
    }

    template< class StateIn >
    void adjust_size( const StateIn & x )
    {
        if( resize_impl( x ) )
            m_first_call = true;
    }

    void reset( void )
    {
        m_first_call = true;
    }

    /**
     * \fn velocity_verlet::initialize( const AccelerationIn &qin )
     * \brief Initializes the internal state of the stepper.
     * \param deriv The acceleration of x. The next call of `do_step` expects that the acceleration of `x` passed to `do_step`
     *              has the value of `qin`.
     */
    template< class AccelerationIn >
    void initialize( const AccelerationIn & ain )
    {
        // alloc a
        m_resizer.adjust_size( ain ,
                               detail::bind( &velocity_verlet::template resize_impl< AccelerationIn > ,
                                             detail::ref( *this ) , detail::_1 ) );
        boost::numeric::odeint::copy( ain , get_current_acc() );
        m_first_call = false;
    }

    template< class System , class CoorIn , class VelocityIn >
    void initialize( System system , const CoorIn & qin , const VelocityIn & pin , time_type t )
    {
        m_resizer.adjust_size( qin ,
                               detail::bind( &velocity_verlet::template resize_impl< CoorIn > ,
                                             detail::ref( *this ) , detail::_1 ) );
        initialize_acc( system , qin , pin , t );
    }

    bool is_initialized( void ) const
    {
        return ! m_first_call;
    }

private:

    template< class System , class CoorIn , class VelocityIn >
    void initialize_acc( System system , const CoorIn & qin , const VelocityIn & pin , time_type t )
    {
        typename odeint::unwrap_reference< System >::type & sys = system;
        sys( qin , pin , get_current_acc() , t );
        m_first_call = false;
    }

    template< class System , class StateInOut >
    void do_step_v1( System system , StateInOut & x , time_type t , time_type dt )
    {
        typedef typename odeint::unwrap_reference< StateInOut >::type state_in_type;
        typedef typename odeint::unwrap_reference< typename state_in_type::first_type >::type coor_in_type;
        typedef typename odeint::unwrap_reference< typename state_in_type::second_type >::type momentum_in_type;

        typedef typename boost::remove_reference< coor_in_type >::type xyz_type;
        state_in_type & statein = x;
        coor_in_type & qinout = statein.first;
        momentum_in_type & pinout = statein.second;

        // alloc a
        if( m_resizer.adjust_size( qinout ,
                                   detail::bind( &velocity_verlet::template resize_impl< xyz_type > ,
                                                 detail::ref( *this ) , detail::_1 ) )
         || m_first_call )
        {
            initialize_acc( system , qinout , pinout , t );
        }

        // check first
        do_step( system , qinout , pinout , get_current_acc() , qinout , pinout , get_old_acc() , t , dt );
        toggle_current_acc();
    }

    template< class StateIn >
    bool resize_impl( const StateIn & x )
    {
        bool resized = false;
        resized |= adjust_size_by_resizeability( m_a1 , x , typename is_resizeable< acceleration_type >::type() );
        resized |= adjust_size_by_resizeability( m_a2 , x , typename is_resizeable< acceleration_type >::type() );
        return resized;
    }

    acceleration_type & get_current_acc( void )
    {
        return m_current_a1 ? m_a1.m_v : m_a2.m_v ;
    }

    const acceleration_type & get_current_acc( void ) const
    {
        return m_current_a1 ? m_a1.m_v : m_a2.m_v ;
    }

    acceleration_type & get_old_acc( void )
    {
        return m_current_a1 ? m_a2.m_v : m_a1.m_v ;
    }

    const acceleration_type & get_old_acc( void ) const
    {
        return m_current_a1 ? m_a2.m_v : m_a1.m_v ;
    }

    void toggle_current_acc( void )
    {
        m_current_a1 = ! m_current_a1;
    }

    resizer_type m_resizer;
    bool m_first_call;
    wrapped_acceleration_type m_a1 , m_a2;
    bool m_current_a1;
};

/**
 * \class velocity_verlet
 * \brief The Velocity-Verlet algorithm.
 *
 * The Velocity-Verlet algorithm is a method for simulation of molecular dynamics systems. It solves the ODE
 * a=f(r,v',t)  where r are the coordinates, v are the velocities and a are the accelerations, hence v = dr/dt, a=dv/dt.
 *
 * \tparam Coor The type representing the coordinates.
 * \tparam Velocity The type representing the velocities.
 * \tparam Value The type value type.
 * \tparam Acceleration The type representing the acceleration.
 * \tparam Time The time representing the independent variable - the time.
 * \tparam TimeSq The time representing the square of the time.
 * \tparam Algebra The algebra.
 * \tparam Operations The operations type.
 * \tparam Resizer The resizer policy type.
 */

    /**
     * \fn velocity_verlet::velocity_verlet( const algebra_type &algebra )
     * \brief Constructs the velocity_verlet class. This constructor can be used as a default
     * constructor if the algebra has a default constructor.
     * \param algebra A copy of algebra is made and stored.
     */

    /**
     * \fn velocity_verlet::do_step( System system , StateInOut &x , time_type t , time_type dt )
     * \brief This method performs one step. It transforms the result in-place.
     *
     * It can be used like
     * \code
     * pair< coordinates , velocities > state;
     * stepper.do_step( sys , x , t , dt );
     * \endcode
     *
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
     *               Second Order System concept.
     * \param x The state of the ODE which should be solved. The state is pair of Coor and Velocity.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

    /**
     * \fn velocity_verlet::do_step( System system , const StateInOut &x , time_type t , time_type dt )
     * \brief This method performs one step. It transforms the result in-place.
     *
     * It can be used like
     * \code
     * pair< coordinates , velocities > state;
     * stepper.do_step( sys , x , t , dt );
     * \endcode
     *
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
     *               Second Order System concept.
     * \param x The state of the ODE which should be solved. The state is pair of Coor and Velocity.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

    /**
     * \fn velocity_verlet::do_step( System system , CoorIn const & qin , VelocityIn const & pin , AccelerationIn const & ain , CoorOut & qout , VelocityOut & pout , AccelerationOut & aout , time_type t , time_type dt )
     * \brief This method performs one step. It transforms the result in-place. Additionally to the other methods
     * the coordinates, velocities and accelerations are passed directly to do_step and they are transformed out-of-place.
     *
     * It can be used like
     * \code
     * coordinates qin , qout;
     * velocities pin , pout;
     * accelerations ain, aout;
     * stepper.do_step( sys , qin , pin , ain , qout , pout , aout , t , dt );
     * \endcode
     *
     * \param system The system function to solve, hence the r.h.s. of the ordinary differential equation. It must fulfill the
     *               Second Order System concept.
     * \param x The state of the ODE which should be solved. The state is pair of Coor and Velocity.
     * \param t The value of the time, at which the step should be performed.
     * \param dt The step size.
     */

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

    /**
     * \fn velocity_verlet::reset( void )
     * \brief Resets the internal state of this stepper. After calling this method it is safe to use all
     * `do_step` method without explicitly initializing the stepper.
     */

    /**
     * \fn velocity_verlet::initialize( System system , const CoorIn &qin , const VelocityIn &pin , time_type t )
     * \brief Initializes the internal state of the stepper.
     *
     * This method is equivalent to
     * \code
     * Acceleration a;
     * system( qin , pin , a , t );
     * stepper.initialize( a );
     * \endcode
     *
     * \param system The system function for the next calls of `do_step`.
     * \param qin The current coordinates of the ODE.
     * \param pin The current velocities of the ODE.
     * \param t The current time of the ODE.
     */

    /**
     * \fn velocity_verlet::is_initialized()
     * \returns Returns if the stepper is initialized.
    */

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

#endif // BOOST_NUMERIC_ODEINT_STEPPER_VELOCITY_VERLET_HPP_DEFINED


//
//   Project Name:        Kratos
//   Last modified by:    $Author: rrossi $
//   Date:                $Date: 2007-08-21 14:11:10 $
//   Revision:            $Revision: 1.3 $
//
//

// System includes

// External includes
#include <boost/python.hpp>

// Project includes
#include "includes/define.h"
#include "processes/process.h"
#include "custom_python/add_custom_utilities_to_python.h"
#include "custom_utilities/FSI_utils.h"
#include "custom_utilities/aitken_utils.h"

namespace Kratos
{

namespace Python
{

  void  AddCustomUtilitiesToPython()
  {
	using namespace boost::python;

	  class_<FSIUtils>("FSIUtils", init<>())
//		.def("FSIUtils",&FSIUtils::GenerateCouplingElements)
		.def("CheckPressureConvergence",&FSIUtils::CheckPressureConvergence)
		.def("StructuralPressurePrediction",&FSIUtils::StructuralPressurePrediction)
		;

	  class_<AitkenUtils>("AitkenUtils", init<>())
		.def("ComputeAitkenFactor",&AitkenUtils::ComputeAitkenFactor)
		.def("ComputeRelaxedDisplacement",&AitkenUtils::ComputeRelaxedDisplacement)
		;
  }

}  // namespace Python.

} // Namespace Kratos


/*
Copyright 2014-2015 Glen Joseph Fernandes
(glenjofe@gmail.com)

Distributed under the Boost Software License, Version 1.0.
(http://www.boost.org/LICENSE_1_0.txt)
*/
#include <boost/align/alignment_of.hpp>
#include <boost/align/is_aligned.hpp>
#include <boost/core/lightweight_test.hpp>
#include <boost/config.hpp>

template<std::size_t N>
struct A { };

template<std::size_t N>
void test(char* p, A<N>)
{
    BOOST_TEST(boost::alignment::is_aligned(p, N));
    BOOST_TEST(!boost::alignment::is_aligned(p + 1, N));
}

void test(char* p, A<1>)
{
    BOOST_TEST(boost::alignment::is_aligned(p, 1));
}

template<class T>
void test()
{
    T o;
    test(reinterpret_cast<char*>(&o),
        A<boost::alignment::alignment_of<T>::value>());
}

class X;

int main()
{
    test<bool>();
    test<char>();
    test<wchar_t>();
#if !defined(BOOST_NO_CXX11_CHAR16_T)
    test<char16_t>();
#endif
#if !defined(BOOST_NO_CXX11_CHAR32_T)
    test<char32_t>();
#endif
    test<short>();
    test<int>();
    test<long>();
#if !defined(BOOST_NO_LONG_LONG) && !defined(_MSC_VER)
    test<long long>();
#endif
    test<float>();
#if !defined(BOOST_MSVC)
    test<double>();
    test<long double>();
#endif
    test<void*>();
    test<char*>();
    test<int*>();
    test<X*>();
    test<void(*)()>();
#if !defined(BOOST_MSVC)
    test<int X::*>();
    test<int(X::*)()>();
#endif

    return boost::report_errors();
}


#ifndef STAN_MATH_PRIM_SCAL_PROB_PARETO_TYPE_2_RNG_HPP
#define STAN_MATH_PRIM_SCAL_PROB_PARETO_TYPE_2_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/scal/err/check_consistent_sizes.hpp>
#include <stan/math/prim/scal/err/check_finite.hpp>
#include <stan/math/prim/scal/err/check_positive_finite.hpp>
#include <stan/math/prim/scal/prob/exponential_rng.hpp>
#include <stan/math/prim/scal/prob/normal_rng.hpp>
#include <boost/random/uniform_real_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/**
 * Return a Pareto type 2 random variate for the given location,
 * scale, and shape using the specified random number generator.
 *
 * mu, lambda, and alpha can each be a scalar or a one-dimensional container.
 * Any non-scalar inputs must be the same size.
 *
 * @tparam T_loc Type of location parameter
 * @tparam T_scale Type of scale parameter
 * @tparam T_shape Type of shape parameter
 * @tparam RNG type of random number generator
 * @param mu (Sequence of) location parameter(s)
 * @param lambda (Sequence of) scale parameter(s)
 * @param alpha (Sequence of) shape parameter(s)
 * @param rng random number generator
 * @return (Sequence of) Pareto type 2 random variate(s)
 * @throw std::domain_error if mu is infinite or lambda or alpha are
 * nonpositive,
 * @throw std::invalid_argument if non-scalar arguments are of different
 * sizes
 */
template <typename T_loc, typename T_scale, typename T_shape, class RNG>
inline typename VectorBuilder<true, double, T_loc, T_scale, T_shape>::type
pareto_type_2_rng(const T_loc& mu, const T_scale& lambda, const T_shape& alpha,
                  RNG& rng) {
  using boost::random::uniform_real_distribution;
  using boost::variate_generator;
  static const char* function = "pareto_type_2_rng";

  check_finite(function, "Location parameter", mu);
  check_positive_finite(function, "Scale parameter", lambda);
  check_positive_finite(function, "Shape parameter", alpha);
  check_consistent_sizes(function, "Location parameter", mu, "Scale Parameter",
                         lambda, "Shape Parameter", alpha);

  scalar_seq_view<T_loc> mu_vec(mu);
  scalar_seq_view<T_scale> lambda_vec(lambda);
  scalar_seq_view<T_shape> alpha_vec(alpha);
  size_t N = max_size(mu, lambda, alpha);
  VectorBuilder<true, double, T_loc, T_scale, T_shape> output(N);

  variate_generator<RNG&, uniform_real_distribution<> > uniform_rng(
      rng, uniform_real_distribution<>(0.0, 1.0));
  for (size_t n = 0; n < N; ++n) {
    output[n] = (std::pow(1.0 - uniform_rng(), -1.0 / alpha_vec[n]) - 1.0)
                    * lambda_vec[n]
                + mu_vec[n];
  }

  return output.data();
}

}  // namespace math
}  // namespace stan
#endif


/* --------------------------------------------------------------------------
*
* (C) Copyright …
*
* ---------------------------------------------------------------------------
*/

/*!
 * @file ReconstructionTester.hpp
 * @date 15/05/2018
 * @author Alessandro Bianco
 */

/*!
 * @addtogroup GuiTests
 *
 * This class will test requirement 4.1.1.5 of deliverable 5.5.
 * "Triangulated points are guaranteed by the algorithm to be placed within the field of view of the camera."
 *
 * @{
 */

#ifndef RECONSTRUCTION_TESTER_HPP
#define RECONSTRUCTION_TESTER_HPP

/* --------------------------------------------------------------------------
 *
 * Includes
 *
 * --------------------------------------------------------------------------
 */
#include <FundamentalMatrixComputation/FundamentalMatrixComputationInterface.hpp>
#include <CamerasTransformEstimation/CamerasTransformEstimationInterface.hpp>
#include <PointCloudReconstruction2DTo3D/PointCloudReconstruction2DTo3DInterface.hpp>
#include <Errors/Assert.hpp>

#include <Types/CPP/Pose.hpp>
#include <Types/CPP/Matrix.hpp>
#include <Types/CPP/CorrespondenceMap2D.hpp>
#include <Types/CPP/PointCloud.hpp>

#include <stdlib.h>
#include <fstream>
#include <string>
#include <vector>
#include <boost/make_shared.hpp>

/* --------------------------------------------------------------------------
 *
 * Class definition
 *
 * --------------------------------------------------------------------------
 */
class ReconstructionTester
	{
	/* --------------------------------------------------------------------
	 * Public
	 * --------------------------------------------------------------------
	 */
	public:
		ReconstructionTester();
		~ReconstructionTester();

		void SetDfns(CDFF::DFN::FundamentalMatrixComputationInterface* fundamentalMatrixEstimator,
				CDFF::DFN::CamerasTransformEstimationInterface* poseEstimator, CDFF::DFN::PointCloudReconstruction2DTo3DInterface* reconstructor);
		void SetConfigurationFilePaths(const std::string& fundamentalMatrixEstimatorFilePath, const std::string& poseEstimatorConfigurationFilePath,
			const std::string& reconstructorConfigurationFilePath);
		void SetInputFilePath(const std::string& inputCorrespodencesFilePath);
		void ExecuteDfns();
		bool AreTriangulatedPointsValid(float fieldOfViewX, float fieldOfViewY);

	/* --------------------------------------------------------------------
	 * Protected
	 * --------------------------------------------------------------------
	 */
	protected:

	/* --------------------------------------------------------------------
	 * Private
	 * --------------------------------------------------------------------
	 */
	private:
		std::string poseEstimatorConfigurationFilePath;
		std::string reconstructorConfigurationFilePath;
		std::string fundamentalMatrixEstimatorFilePath;
		std::string inputCorrespodencesFilePath;

		CorrespondenceMap2DWrapper::CorrespondenceMap2DConstPtr inputCorrespondenceMap;
		MatrixWrapper::Matrix3dConstPtr fundamentalMatrix;
		bool fundamentalMatrixSuccess;
		PoseWrapper::Transform3DConstPtr cameraTransform;
		bool poseEstimatorWasSuccessful;
		PointCloudWrapper::PointCloudConstPtr outputPointCloud;

		CDFF::DFN::FundamentalMatrixComputationInterface* fundamentalMatrixEstimator;
		CDFF::DFN::CamerasTransformEstimationInterface* poseEstimator;
		CDFF::DFN::PointCloudReconstruction2DTo3DInterface* reconstructor;

		bool dfnsWereLoaded;
		bool inputCorrespondencesWereLoaded;
		bool dfnsWereExecuted;

		void LoadInputCorrespondences();
		void ConfigureDfns();
		bool AllPointsAreInTheFieldOfView(float fieldOfViewX, float fieldOfViewY);
	};

#endif

/* ReconstructionTester.hpp */
/** @} */


/**
 * sudoku_tl.cpp
 *
 * By Sebastian Raaphorst, 2018.
 */

#include <iostream>
#include <memory>
#include <stdexcept>

#include <boost/program_options.hpp>

#include <GenSudokuBoard.h>
#include <GenSudokuBoardAscenderPopulator.h>
#include <TabuSearchAlgorithm.h>

#include "progopts.h"

namespace po = boost::program_options;
using namespace vorpal::gensudoku;
using namespace vorpal::stochastic;

int main(int argc, const char * const argv[]) {
    using solver = TabuSearchAlgorithm<SudokuBoard>;
    solver::options_type options;

    try {
        auto desc = init_options(options);
        desc->add_options()
                ("tabu-list-length,t", po::value<size_t>()->default_value(20), "tabu list length");
        auto done = end_options<SudokuBoardAscenderPopulator, solver::options_type>(argc, argv, desc, options,
        [](po::variables_map &vm, solver::options_type &options) {
            options.tabu_list_length = vm["tabu-list-length"].as<size_t>();
        });
        if (done) return 0;
    } catch (const std::exception &e) {
        std::cerr << e.what() << '\n';
        return 1;
    }

    run<solver>(options);
}


/*
@copyright Louis Dionne 2014
Distributed under the Boost Software License, Version 1.0.
(See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
 */

#include <boost/hana/constant/mcd.hpp>

#include <boost/hana/detail/minimal/constant.hpp>
using namespace boost::hana;

template <typename mcd, typename T>
struct test {
    static_assert(value(detail::minimal::constant<T, 0, mcd>) == 0, "");
    static_assert(value(detail::minimal::constant<T, 1, mcd>) == 1, "");
};

template struct test<Constant::mcd, int>;
template struct test<Constant::mcd, long>;

int main() { }


// Copyright  (C)  2021 Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>

// Version: 1.0
// Author: Djordje Vukcevic <djordje dot vukcevic at h-brs dot de>
// URL: http://www.orocos.org/kdl

// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.

// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.

// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

#ifndef KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP
#define KDL_CHAIN_EXTERNAL_WRENCH_ESTIMATOR_HPP

#include <Eigen/Core>
#include "utilities/svd_eigen_HH.hpp"
#include "chaindynparam.hpp"
#include "chainjnttojacsolver.hpp"
#include "chainfksolverpos_recursive.hpp"
#include <iostream>

namespace KDL {

    /**
     * \brief First-order momentum observer for the estimation of external wrenches applied on the robot's end-effector.
     *
     * Implementation based on:
     * S. Haddadin, A. De Luca and A. Albu-Schäffer,
     * "Robot Collisions: A Survey on Detection, Isolation, and Identification,"
     * in IEEE Transactions on Robotics, vol. 33(6), pp. 1292-1312, 2017.
     *
     * Note: This component assumes that the external wrench is applied on the end-effector (last) link of the robot's chain.
     */
    class ChainExternalWrenchEstimator : public SolverI
    {
        typedef Eigen::Matrix<double, 6, 1 > Vector6d;

    public:

        static const int E_FKSOLVERPOS_FAILED = -100; //! Internally-used Forward Position Kinematics (Recursive) solver failed
        static const int E_JACSOLVER_FAILED = -101; //! Internally-used Jacobian solver failed
        static const int E_DYNPARAMSOLVERMASS_FAILED = -102; //! Internally-used Dynamics Parameters (Mass) solver failed
        static const int E_DYNPARAMSOLVERCORIOLIS_FAILED = -103; //! Internally-used Dynamics Parameters (Coriolis) solver failed
        static const int E_DYNPARAMSOLVERGRAVITY_FAILED = -104; //! Internally-used Dynamics Parameters (Gravity) solver failed

        /**
         * Constructor for the estimator, it will allocate all the necessary memory
         * \param chain The kinematic chain of the robot, an internal copy will be made.
         * \param gravity The gravity-acceleration vector to use during the calculation.
         * \param sample_frequency Frequency at which users updates it estimation loop (in Hz).
         * \param estimation_gain Parameter used to control the estimator's convergence
         * \param filter_constant Parameter defining how much the estimated signal should be filtered by the low-pass filter.
         *                        This input value should be between 0 and 1. Higher the number means more noise needs to be filtered-out.
         *                        The filter can be turned off by setting this value to 0.
         * \param eps If a SVD-singular value is below this value, its inverse is set to zero. Default: 0.00001
         * \param maxiter Maximum iterations for the SVD computations. Default: 150.
         */
        ChainExternalWrenchEstimator(const Chain &chain, const Vector &gravity, const double sample_frequency, const double estimation_gain, const double filter_constant, const double eps = 0.00001, const int maxiter = 150);
        ~ChainExternalWrenchEstimator(){};

        /**
         * Calculates robot's initial momentum in the joint space.
         * Bassically, sets the offset for future estimation (momentum calculation).
         * If this method is not called by the user, zero values will be taken for the initial momentum.
         */
        int setInitialMomentum(const JntArray &joint_position, const JntArray &joint_velocity);

        // Sets singular-value eps parameter for the SVD calculation
        void setSVDEps(const double eps_in);

        // Sets maximum iteration parameter for the SVD calculation
        void setSVDMaxIter(const int maxiter_in);

        /**
         * This method calculates the external wrench that is applied on the robot's end-effector.
         * Input parameters:
         * \param joint_position The current (measured) joint positions.
         * \param joint_velocity The current (measured) joint velocities.
         * \param joint_torque The joint space torques.
         *                     Depending on the user's choice, this array can represent commanded or measured joint torques.
         *                     A particular choice depends on the available sensors in robot's joint.
         *                     For more details see the above-referenced article.
         *
         * Output parameters:
         * \param external_wrench The estimated external wrench applied on the robot's end-effector.
         *                        The wrench will be expressed w.r.t. end-effector's frame.
         *
         * @return error/success code
         */
        int JntToExtWrench(const JntArray &joint_position, const JntArray &joint_velocity, const JntArray &joint_torque, Wrench &external_wrench);

        // Returns the torques felt in the robot's joints as a result of the external wrench being applied on the robot.
        void getEstimatedJntTorque(JntArray &external_joint_torque);

        /// @copydoc KDL::SolverI::updateInternalDataStructures()
        virtual void updateInternalDataStructures();

        /// @copydoc KDL::SolverI::strError()
        virtual const char* strError(const int error) const;

    private:
        const Chain &CHAIN;
        const double DT_SEC, FILTER_CONST;
        double svd_eps;
        int svd_maxiter;
        unsigned int nj, ns;
        JntSpaceInertiaMatrix jnt_mass_matrix, previous_jnt_mass_matrix, jnt_mass_matrix_dot;
        JntArray initial_jnt_momentum, estimated_momentum_integral, filtered_estimated_ext_torque,
                 gravity_torque, coriolis_torque, total_torque, estimated_ext_torque;
        Jacobian jacobian_end_eff;
        Eigen::MatrixXd jacobian_end_eff_transpose, jacobian_end_eff_transpose_inv, U, V;
        Eigen::VectorXd S, S_inv, tmp, ESTIMATION_GAIN;
        ChainDynParam dynparam_solver;
        ChainJntToJacSolver jacobian_solver;
        ChainFkSolverPos_recursive fk_pos_solver;
    };
}

#endif


#include <iostream>
#include <nt2/sdk/details/type_id.hpp>
#include <nt2/sdk/memory/lead_padding.hpp>

#include <boost/fusion/include/nview.hpp>
#include <nt2/core/container/details/block_data.hpp>

using namespace std;

namespace nt2 { namespace container
{
  // CPU Heap based block
  template< class Type
          , class Extend
          , class Bases
          , class Sizes
          , class StorageOrder
          , class Allocator
          , class Padding
          >
  struct  block<Type,Extend,Bases,Sizes,StorageOrder,heap_(Allocator),Padding>
        : details::block_extent<Bases,Sizes>
        , details::block_data<Type,heap_(Allocator),Extend>
  {
    typedef details::block_extent<Bases,Sizes>                extent_parent;
    typedef details::block_data<Type,heap_(Allocator),Extend> data_parent;

    ////////////////////////////////////////////////////////////////////////////
    // block is a RandonAccessSequence
    ////////////////////////////////////////////////////////////////////////////
    typedef Type       value_type;
//    typedef typename buffer_type::pointer          iterator;
//    typedef typename buffer_type::const_pointer    const_iterator;
//    typedef typename allocator_type::reference        reference;
//    typedef typename allocator_type::const_reference  const_reference;
    typedef typename extent_parent::size_type        size_type;
    typedef typename extent_parent::difference_type  difference_type;
    typedef typename extent_parent::difference_type  index_type;

    ////////////////////////////////////////////////////////////////////////////
    // Constructors and stuff
    ////////////////////////////////////////////////////////////////////////////
    block(Bases const& bs, Sizes const& sz, Allocator const& a = Allocator())
          : extent_parent(bs,sz)
    {
      init();
      link();
    }

    protected:
    ////////////////////////////////////////////////////////////////////////////
    // Initialize a NRC block
    ////////////////////////////////////////////////////////////////////////////
    using data_parent::init;

    void init()
    {
      boost::fusion::nview<Sizes const,StorageOrder>  sz(extent_parent::mSize);
      boost::fusion::nview<Bases const,StorageOrder>  bz(extent_parent::mBase);
      init( Extend(), bz, sz, Padding()
          , typename meta::is_composite<Type>::type()
          );
    }

    ////////////////////////////////////////////////////////////////////////////
    // Link indexes and data, NRC style and recursively
    ////////////////////////////////////////////////////////////////////////////
    using data_parent::link;
    void link()
    {
      boost::fusion::nview<Sizes const,StorageOrder>  sz(extent_parent::mSize);
      link(Extend(), sz, Padding(), typename meta::is_composite<Type>::type());
    }
  };
} }

struct foo {float x; double y; };

NT2_ADAPT_COMPOSITE(foo, (float, x)(double, y));

using nt2::container::block;

int main()
{
  {
    typedef block < float
                  , boost::mpl::int_<1>
                  , boost::mpl::vector_c<int,9>
                  , boost::array<int,1>
                  , boost::mpl::vector_c<int,0>
                  , nt2::heap_(nt2::memory::allocator<float>)
                  , nt2::memory::lead_padding
                  > t_t;

    boost::array<int,1> sz= {{5}};
    boost::mpl::vector_c<int,9> bz;
    t_t  t(bz,sz);

    cout << t.size() << "\n";
    cout  << t.lower<1>()
          << " " << t.upper<1>()
          << " " << t.size<1>()
          << "\n";
  }

  {
    typedef block < float
                  , boost::mpl::int_<2>
                  , boost::mpl::vector_c<int,3,1>
                  , boost::array<int,2>
                  , boost::mpl::vector_c<int,0,1>
                  , nt2::heap_(nt2::memory::allocator<float>)
                  , nt2::memory::lead_padding
                  > t_t;
    boost::array<int,2> sz= {{5,3}};
    boost::mpl::vector_c<int,3,1> bz;
    t_t  t(bz,sz);

    cout << t.size() << "\n";
    cout  << t.lower<1>()
          << " " << t.upper<1>()
          << " " << t.size<1>()
          << "\n";
    cout  << t.lower<2>()
          << " " << t.upper<2>()
          << " " << t.size<2>()
          << "\n";
  }

  {
    typedef block < foo
                  , boost::mpl::int_<1>
                  , boost::mpl::vector_c<int,0>
                  , boost::array<int,1>
                  , boost::mpl::vector_c<int,0>
                  , nt2::heap_(nt2::memory::allocator<foo>)
                  , nt2::memory::lead_padding
                  > t_t;
    boost::array<int,1> sz= {{5}};
    boost::mpl::vector_c<int,0> bz;
    t_t  t(bz,sz);

    cout << t.size() << "\n";
    cout  << t.lower<1>()
          << " " << t.upper<1>()
          << " " << t.size<1>()
          << "\n";
  }

  {
    typedef block < foo
                   , boost::mpl::int_<2>
                   , boost::mpl::vector_c<int,0,0>
                   , boost::array<int,2>
                   , boost::mpl::vector_c<int,0,1>
                   , nt2::heap_(nt2::memory::allocator<foo>)
                   , nt2::memory::lead_padding
                   > t_t;
     boost::array<int,2> sz= {{5,7}};
     boost::mpl::vector_c<int,0,0> bz;
     t_t  t(bz,sz);

     cout << t.size() << "\n";
     cout  << t.lower<1>()
           << " " << t.upper<1>()
           << " " << t.size<1>()
           << "\n";
  }
}


// Copyright (C) 2016-2018 T. Zachary Laine
//
// 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)
#include <boost/yap/expression.hpp>

#include <boost/test/minimal.hpp>

template<typename T>
using term = boost::yap::terminal<boost::yap::expression, T>;

template<typename T>
using ref = boost::yap::expression_ref<boost::yap::expression, T>;

namespace yap = boost::yap;
namespace bh = boost::hana;

int test_main(int, char * [])
{
    {
        term<double> unity{1.0};
        int i_ = 42;
        term<int &&> i{std::move(i_)};
        yap::expression<
            yap::expr_kind::minus,
            bh::tuple<ref<term<double> &>, term<int &&>>>
            expr = unity - std::move(i);
        yap::expression<
            yap::expr_kind::plus,
            bh::tuple<
                ref<term<double> &>,
                yap::expression<
                    yap::expr_kind::minus,
                    bh::tuple<ref<term<double> &>, term<int &&>>>>>
            unevaluated_expr_1 = unity + std::move(expr);

        yap::expression<
            yap::expr_kind::plus,
            bh::tuple<ref<term<double> &>, ref<term<double> &>>>
            unevaluated_expr_2 = unity + unity;

        term<double> const const_unity{1.0};
        yap::expression<
            yap::expr_kind::plus,
            bh::tuple<ref<term<double> &>, ref<term<double> const &>>>
            unevaluated_expr_3 = unity + const_unity;

        {
            double result = evaluate(unity);
            BOOST_CHECK(result == 1);
        }

        {
            double result = evaluate(expr);
            BOOST_CHECK(result == -41);
        }

        {
            double result = evaluate(unevaluated_expr_1);
            BOOST_CHECK(result == -40);
        }

        {
            double result = evaluate(unevaluated_expr_2);
            BOOST_CHECK(result == 2);
        }

        {
            double result = evaluate(unevaluated_expr_3);
            BOOST_CHECK(result == 2);
        }

        {
            double result = evaluate(unity, 5, 6, 7);
            BOOST_CHECK(result == 1);
        }

        {
            double result = evaluate(expr);
            BOOST_CHECK(result == -41);
        }

        {
            double result = evaluate(unevaluated_expr_1, std::string("15"));
            BOOST_CHECK(result == -40);
        }

        {
            double result = evaluate(unevaluated_expr_2, std::string("15"));
            BOOST_CHECK(result == 2);
        }

        {
            double result = evaluate(unevaluated_expr_3, std::string("15"));
            BOOST_CHECK(result == 2);
        }
    }

    return 0;
}


/*********************************************************************************
 * Copyright (c) 2018, Università degli Studi di Salerno, ALTEC S.p.A.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 *    list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its
 *    contributors may be used to endorse or promote products derived from
 *    this software without specific prior written permission.
 *
 * 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 HOLDER 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.
 *********************************************************************************/

/* -------------------------------------------------------------------
 *
 * This module has been developed as part of a collaboration between
 * the Automatic Control Group @ UNISA and ALTEC.
 *
 * Title:   objective_function.cpp
 * Author:  Enrico Ferrentino
 * Org.:    UNISA - Automatic Control Group
 * Date:    Nov 21, 2018
 *
 * See objective_function.h for a description of the class.
 *
 * -------------------------------------------------------------------
 */

#include <moveit_dp_redundancy_resolution/objective_function.h>
#include <moveit_dp_redundancy_resolution/exceptions.h>
#include <boost/property_tree/xml_parser.hpp>
#include <boost/foreach.hpp>

const std::map<std::string, int> moveit_dp_redundancy_resolution::ObjectiveFunction::PERFORMANCE_INDEX_TYPE_MAP =
        {{"square_norm_velocities",     0},
        {"frame_obstacle_distance",     1}};

moveit_dp_redundancy_resolution::ObjectiveFunction::ObjectiveFunction(const std::string & description_file)
{
    boost::property_tree::ptree tree;
    boost::property_tree::read_xml(description_file, tree);

    try{

        std::string criterion_str = tree.get<std::string>("objective_function.<xmlattr>.criterion");

        if(criterion_str == "minimize")
            criterion_ = OptimizationCriterion::minimize;
        else
            if(criterion_str == "maximize")
                criterion_ = OptimizationCriterion::maximize;
            else
                moveit_dp_redundancy_resolution::Exception("Unrecognized optimization criterion: it must be either 'minimize' or 'maximize'");

    }catch(boost::property_tree::ptree_bad_path & e)
    {
        ROS_ERROR("%s", e.what());
        throw moveit_dp_redundancy_resolution::Exception("The objective_function tag must exist and must have a 'criterion' attribute");
    }

    double total_weight = 0;

    BOOST_FOREACH(boost::property_tree::ptree::value_type const& v, tree.get_child("objective_function"))
    {
        if(v.first == "performance_index")
        {
            try{
                std::string pi_type = v.second.get<std::string>("<xmlattr>.type");
                PerformanceIndex * pi;

                switch(PERFORMANCE_INDEX_TYPE_MAP.at(pi_type))
                {
                case 0:
                    {
                        pi = new SquareNormVelocities(v.second);
                        p_indices_.push_back(pi);
                        break;
                    }
                case 1:
                    {
                        pi = new FrameObstacleDistance(v.second);
                        p_indices_.push_back(pi);
                        break;
                    }
                default:
                    throw moveit_dp_redundancy_resolution::Exception("Unrecognized performance index type");

                }

            }catch(boost::property_tree::ptree_bad_path & e)
            {
                ROS_ERROR("%s", e.what());
                throw moveit_dp_redundancy_resolution::Exception("Each performance_index tag must have a 'type' attribute");
            }

            total_weight += p_indices_[p_indices_.size()-1]->getWeight();
        }
    }

    if(total_weight != 1.0)
        throw moveit_dp_redundancy_resolution::Exception("Performance indices weights do not sum to one");

}

moveit_dp_redundancy_resolution::ObjectiveFunction::~ObjectiveFunction()
{
    std::vector<PerformanceIndex *>::iterator iter = p_indices_.begin();

    for(; iter != p_indices_.end(); iter++)
        delete *iter;
}

bool moveit_dp_redundancy_resolution::ObjectiveFunction::isCriterionMinimize() const
{
    if(criterion_ == OptimizationCriterion::minimize)
        return true;
    else
        return false;
}

bool moveit_dp_redundancy_resolution::ObjectiveFunction::isCriterionMaximize() const
{
    return !isCriterionMinimize();
}

double moveit_dp_redundancy_resolution::ObjectiveFunction::computeLocalCost(
        const robot_state::RobotState * rs,
        const moveit::core::JointModelGroup * jmg,
        double time_step) const
{
    std::vector<PerformanceIndex *>::const_iterator iter = p_indices_.begin();

    double cost = 0;

    for(; iter != p_indices_.end(); iter++)
    {
        cost += (*iter)->getWeight() * (*iter)->computeLocalCost(rs, jmg, time_step);
    }

    return cost;
}

moveit_dp_redundancy_resolution::PerformanceIndex::PerformanceIndex(const boost::property_tree::ptree & tree)
{
    try{

        weight_ = tree.get<double>("<xmlattr>.weight");

    }catch(boost::property_tree::ptree_bad_path & e)
    {
        ROS_ERROR("%s", e.what());
        throw moveit_dp_redundancy_resolution::Exception("Each performance_index tag must have a 'weight' attribute");
    }
}

double moveit_dp_redundancy_resolution::PerformanceIndex::getWeight() const
{
    return weight_;
}

moveit_dp_redundancy_resolution::SquareNormVelocities::SquareNormVelocities(const boost::property_tree::ptree & tree):
        PerformanceIndex(tree)
{
}

double moveit_dp_redundancy_resolution::SquareNormVelocities::computeLocalCost(
        const robot_state::RobotState * rs,
        const moveit::core::JointModelGroup * jmg,
        double time_step) const
{
    Eigen::VectorXd qd_curr;

    rs->copyJointGroupVelocities(jmg, qd_curr);

    return time_step * qd_curr.dot(qd_curr);
}

moveit_dp_redundancy_resolution::FrameObstacleDistance::FrameObstacleDistance(const boost::property_tree::ptree & tree):
        PerformanceIndex(tree)
{
    try{

        frame_name_ = tree.get<std::string>("frame");
        obstacle_pos_ <<  tree.get<double>("obstacle.x"), tree.get<double>("obstacle.y"), tree.get<double>("obstacle.z");

    }catch(boost::property_tree::ptree_bad_path & e)
    {
        ROS_ERROR("%s", e.what());
        throw moveit_dp_redundancy_resolution::Exception("The frame_obstacle_distance performance index requires the name of a frame fixed to the robot and the coordinates of the obstacle (x,y,z) in the world reference frame");
    }
}

double moveit_dp_redundancy_resolution::FrameObstacleDistance::computeLocalCost(
        const robot_state::RobotState * rs,
        const moveit::core::JointModelGroup * jmg,
        double time_step) const
{
    std::vector<std::string> names = jmg->getActiveJointModelNames();

    const Eigen::Affine3d & transform = rs->getFrameTransform(frame_name_);

    Eigen::Vector3d joint_pos = transform.translation();

    Eigen::Vector3d distance = joint_pos - obstacle_pos_;

    return time_step * distance.dot(distance);

}


// Copyright (c) 2019 Graphcore Ltd. All rights reserved.
#define BOOST_TEST_MODULE NoGradOpTest

#include <boost/test/unit_test.hpp>

#include <memory>

#include <test_runner.hpp>

#include <onnxutil.hpp>
#include <popart/builder.hpp>
#include <popart/devicemanager.hpp>
#include <popart/logging.hpp>
#include <popart/ndarraywrapper.hpp>
#include <popart/op.hpp>
#include <popart/op/identity.hpp>
#include <popart/op/l1.hpp>
#include <popart/op/varupdate.hpp>
#include <popart/opmanager.hpp>
#include <popart/patterns/pattern.hpp>
#include <popart/popx/devicex.hpp>
#include <popart/popx/opx.hpp>
#include <popart/popx/opxmanager.hpp>
#include <popart/session.hpp>
#include <popart/tensordata.hpp>
#include <popart/tensorinfo.hpp>
#include <popart/tensornames.hpp>

#include <popops/ElementWise.hpp>

using namespace popart;

namespace CustomOperators {
const OperatorIdentifier DontTrain = {"com.acme", "DontTrain", 1};
} // namespace CustomOperators

// An IdentityOp that doesn't return any grad ops.
class DontTrainOp : public Op {
public:
  DontTrainOp(const OperatorIdentifier &_opid, const Op::Settings &settings_)
      : Op(_opid, settings_) {}

  void setup() final { outInfo(0) = inInfo(0); }

  std::unique_ptr<Op> clone() const final {
    return std::make_unique<DontTrainOp>(*this);
  }

  float getSubgraphValue() const final { return getLowSubgraphValue(); }
};

static popart::OpDefinition dontTrainOpDef(
    {popart::OpDefinition::Inputs({
         {"input", {{popart::DataType::FLOAT, popart::DataType::FLOAT16}}},
     }),
     popart::OpDefinition::Outputs(
         {{"output", {{popart::DataType::FLOAT, popart::DataType::FLOAT16}}}}),
     popart::OpDefinition::Attributes({})});

static OpCreator<DontTrainOp>
    donttrainOpCreator({{CustomOperators::DontTrain, dontTrainOpDef}});

class DontTrainOpx : public popx::Opx {
public:
  DontTrainOpx(Op *op, popx::Devicex *devicex) : popx::Opx(op, devicex) {
    verifyOp<DontTrainOp>(op, CustomOperators::DontTrain);
  }

  void grow(poplar::program::Sequence &prog) const final {
    insert(outId(0), cloneNcopy(prog, getInTensor(0)));
  }
};

static popx::OpxCreator<DontTrainOpx>
    donttrainOpxCreator(CustomOperators::DontTrain);

// a = conv(in, w0)
// b = conv(in, w1)
// c = donttrain(b)
// d = add(a, c)
BOOST_AUTO_TEST_CASE(Basic0) {
  auto nChans         = 1;
  auto batchSize      = 1;
  auto convHeightWith = 2;

  TensorInfo inputInfo{
      "FLOAT",
      std::vector<int64_t>{batchSize, nChans, convHeightWith, convHeightWith}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  std::vector<float> convWeights0(nChans * nChans, 0);
  for (int i = 0; i < convWeights0.size(); i++) {
    convWeights0[i] = 0.25f;
  }
  std::vector<float> convWeights1 = convWeights0;

  TensorInfo convWeightInfo{"FLOAT",
                            std::vector<int64_t>{nChans, nChans, 1, 1}};

  ConstVoidData convData0{convWeights0.data(), convWeightInfo};
  ConstVoidData convData1{convWeights1.data(), convWeightInfo};

  TensorId conv1WeightsInput;

  TestRunner runner;
  runner.isTraining = true;

  runner.buildModel([&](auto &builder) {
    auto aiOnnx = builder.aiOnnxOpset9();

    auto input             = builder.addInputTensor(inputInfo);
    auto conv0WeightsInput = builder.addInitializedInputTensor(convData0);
    conv1WeightsInput      = builder.addInitializedInputTensor(convData1);

    auto ident0 = aiOnnx.identity({input});
    auto conv0  = aiOnnx.conv(
        {ident0, conv0WeightsInput}, {1, 1}, 1, {}, {0, 0, 0, 0}, {1, 1});
    auto conv1 = aiOnnx.conv(
        {ident0, conv1WeightsInput}, {1, 1}, 1, {}, {0, 0, 0, 0}, {1, 1});
    auto donttrain0 =
        builder.customOp(CustomOperators::DontTrain, 1, {conv0}, 1, {}).at(0);
    auto add0   = aiOnnx.add({donttrain0, conv1});
    auto output = aiOnnx.identity({add0});
    auto l1     = builder.aiGraphcoreOpset1().l1loss({output}, 0.1);

    inputs.push_back(
        TestTensor::create<float>(input, {1, 2, 3, 4}, inputInfo.shape()));
    outputs.push_back(TestTensor::create<float>(output, inputInfo.shape()));

    runner.loss = l1;

    return output;
  });

  runner.checkIr([&](Ir &ir) {
    // There should only be one SGD0VarUpdateOp
    auto varUpdates = ir.opsOfType(Onnx::CustomOperators::SGD0VarUpdate);
    BOOST_CHECK_EQUAL(varUpdates.size(), 1);
    // And it should be to update conv1WeightsInput
    BOOST_CHECK_EQUAL(
        varUpdates.at(0)->inId(VarUpdateOp::getVarToUpdateInIndex()),
        conv1WeightsInput);
  });

  runner.checkResult(
      [&](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        logging::debug("result: {}", data);
      },
      inputs,
      outputs);
}

// a = conv(in, w)
// b = donttrain(a)
// c = add(a, b)
BOOST_AUTO_TEST_CASE(Basic1) {
  auto nChans         = 1;
  auto batchSize      = 1;
  auto convHeightWith = 2;

  TensorInfo inputInfo{
      "FLOAT",
      std::vector<int64_t>{batchSize, nChans, convHeightWith, convHeightWith}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  std::vector<float> convWeights(nChans * nChans, 0);
  for (int i = 0; i < convWeights.size(); i++) {
    convWeights[i] = 0.25f;
  }

  TensorInfo convWeightInfo{"FLOAT",
                            std::vector<int64_t>{nChans, nChans, 1, 1}};

  ConstVoidData convData{convWeights.data(), convWeightInfo};

  TestRunner runner;
  runner.isTraining = true;

  TensorId convWeightsInput;

  runner.buildModel([&](auto &builder) {
    auto aiOnnx = builder.aiOnnxOpset9();

    auto input       = builder.addInputTensor(inputInfo);
    convWeightsInput = builder.addInitializedInputTensor(convData);

    auto ident0 = aiOnnx.identity({input});
    auto conv0  = aiOnnx.conv(
        {ident0, convWeightsInput}, {1, 1}, 1, {}, {0, 0, 0, 0}, {1, 1});
    auto donttrain0 =
        builder.customOp(CustomOperators::DontTrain, 1, {conv0}, 1, {}).at(0);
    auto add0   = aiOnnx.add({donttrain0, conv0});
    auto output = aiOnnx.identity({add0});
    auto l1     = builder.aiGraphcoreOpset1().l1loss({output}, 0.1);

    inputs.push_back(
        TestTensor::create<float>(input, {1, 2, 3, 4}, inputInfo.shape()));
    outputs.push_back(TestTensor::create<float>(output, inputInfo.shape()));

    runner.loss = l1;

    return output;
  });

  runner.checkIr([&](Ir &ir) {
    // There should only be a SGD0VarUpdateOp
    auto varUpdates = ir.opsOfType(Onnx::CustomOperators::SGD0VarUpdate);
    BOOST_CHECK_EQUAL(varUpdates.size(), 1);
    // And it should be to update conv1WeightsInput
    BOOST_CHECK_EQUAL(
        varUpdates.at(0)->inId(VarUpdateOp::getVarToUpdateInIndex()),
        convWeightsInput);
  });

  runner.checkResult(
      [&](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        logging::debug("result: {}", data);
      },
      inputs,
      outputs);
}

// w0, w1 = split(w)
// a      = conv(in, w0)
// b      = donttrain(conv(in, w1))
// c      = add(a, b)
BOOST_AUTO_TEST_CASE(Basic2) {
  auto nChans         = 1;
  auto batchSize      = 1;
  auto convHeightWith = 2;

  TensorInfo inputInfo{
      "FLOAT",
      std::vector<int64_t>{batchSize, nChans, convHeightWith, convHeightWith}};
  std::vector<TestTensor> inputs;
  std::vector<TestTensor> outputs;

  TensorInfo convWeightInfo{"FLOAT",
                            std::vector<int64_t>{nChans * 2, nChans, 1, 1}};

  std::vector<float> convWeights(convWeightInfo.nelms(), 0);
  for (int i = 0; i < convWeights.size(); i++) {
    convWeights[i] = 0.25f;
  }

  ConstVoidData convData{convWeights.data(), convWeightInfo};

  TestRunner runner;
  runner.isTraining = true;

  TensorId convWeightsInput;

  runner.buildModel([&](auto &builder) {
    auto aiOnnx = builder.aiOnnxOpset9();

    auto input       = builder.addInputTensor(inputInfo);
    convWeightsInput = builder.addInitializedInputTensor(convData);

    auto splits = aiOnnx.split({convWeightsInput}, 2, 0, {nChans, nChans});
    auto cw0    = splits.at(0);
    auto cw1    = splits.at(1);

    auto conv0 = aiOnnx.conv({input, cw0}, {1, 1}, 1, {}, {0, 0, 0, 0}, {1, 1});

    auto conv1 = aiOnnx.conv({input, cw1}, {1, 1}, 1, {}, {0, 0, 0, 0}, {1, 1});
    auto donttrain0 =
        builder.customOp(CustomOperators::DontTrain, 1, {conv1}, 1, {}).at(0);

    auto output = aiOnnx.add({conv0, donttrain0});
    auto l1     = builder.aiGraphcoreOpset1().l1loss({output}, 0.1);

    inputs.push_back(
        TestTensor::create<float>(input, {1, 2, 3, 4}, inputInfo.shape()));
    outputs.push_back(TestTensor::create<float>(output, inputInfo.shape()));

    runner.loss = l1;

    return output;
  });

  runner.checkIr([&](Ir &ir) {
    // There should be no as a grad edge was not provided for every input of
    // splits grad op.
    auto varUpdates = ir.opsOfType(Onnx::CustomOperators::SGD0VarUpdate);
    BOOST_CHECK_EQUAL(varUpdates.size(), 0);
  });

  runner.checkResult(
      [&](TestTensor &result) {
        auto data = result.getDataCopy<float>();
        logging::debug("result: {}", data);
      },
      inputs,
      outputs);
}