#include <ScrimpTrivialPar.hpp>
#include <logging.hpp>
#include <timing.h>
#include <partitioning_1d.h>
#include <binproffile.h>

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <iomanip>
#include <limits>
#include <algorithm>
#include <string>
#include <sstream>
#include <cassert>
#include <memory>
#include <cstddef>
#include <kernels.h>

#include <boost/assert.hpp>
#include <boost/align.hpp>
#include <boost/filesystem.hpp>

#include <mpi.h>

#include <papiwrapper.hpp>

using namespace matrix_profile;

using DiagPartition = Partition1D;

// easy aligned allocation.
// code from https://www.boost.org/doc/libs/1_57_0/doc/html/align/examples.html#align.examples.aligned_ptr, licensed under the Boost Software License - Version 1.0
// see copy of the license terms provided under /licenses/boostSoftwareLicense.txt
template<class T>
using aligned_ptr = std::unique_ptr<T,
  boost::alignment::aligned_delete>;

template<class T, class... Args>
inline aligned_ptr<T> make_aligned(Args&&... args)
{
  auto p = boost::alignment::
    aligned_alloc(alignof(T), sizeof(T));
  if (!p) {
	throw std::bad_alloc();
  }
  try {
	auto q = ::new(p) T(std::forward<Args>(args)...);
	return aligned_ptr<T>(q);
  } catch (...) {
	boost::alignment::aligned_free(p);
	throw;
  }
}
// condinue own code

#define TIMEPOINT( varname ) const Timepoint varname = matrix_profile::get_cur_time();

using namespace matrix_profile;

static FactoryRegistration<ScrimpTrivialPar> s_trivParReg("scrimp_triv_par");
static const int notification_interval_iter = 10000;

void ScrimpTrivialPar::MPI_MatProfSOA_reduction(void * invec, void *inoutvec, int *len, MPI_Datatype *datatype) {
	MatProfSOA* inout = static_cast<MatProfSOA*>(inoutvec);
	const MatProfSOA* const in2 = static_cast<const MatProfSOA* const>(invec);

	EXEC_TRACE("reducing a MatProfSOA!");
	// retrieve the profile length: This is equal to the block length of the datastructure, not the length specified in the reduction!
	int num_int, num_add, num_dtype, combiner;
	MPI_Type_get_envelope(*datatype, &num_int, &num_add ,&num_dtype, &combiner);

	if (num_int>3 || num_add>3 || num_dtype >3){
		EXEC_ERROR("Invalid dataype sizes retrieved: num_i " << num_int
		           << " num_add " << num_add
		           << " num_dtype " << num_dtype
		           << "aborting as buffers are too small");
		MPI_Abort(MPI_COMM_WORLD, 1);
	}

	int blocklens[3];
	MPI_Aint addresses[3];
	MPI_Datatype dtypes[3];
	MPI_Type_get_contents(*datatype, num_int, num_add, num_dtype, blocklens, addresses, dtypes);

	EXEC_DEBUG("reduction with len " << *len << " and blocklen " << blocklens[1] <<", " << blocklens[2]);
	assert(blocklens[1] == blocklens[2]);
	assert(*len == 1); // only reduction with size 1 is supported: Otherwise strides between SOAs needed to be considered....

	//"shortcuts" for coding convenience
	tsa_dtype* inoutprof = inout->profile.data();
	idx_dtype* inout_idx = inout->index.data();
	const tsa_dtype* const in2_prof = in2->profile.data();
	const idx_dtype* const in2_idx = in2->index.data();

	//finally the reduction
	const idx_dtype limit = blocklens[1]; // avoid unnecessary reads
	for (idx_dtype i = 0; i < limit; ++i) {
		if (in2_prof[i] < inoutprof[i]) {
			inoutprof[i] = in2_prof[i];
			inout_idx[i] = in2_idx[i];
		}
	}
}

void ScrimpTrivialPar::eval_diagonal_block(MatProfSOA& result,
           const int blocklen,
           const aligned_tsdtype_vec& A,
           aligned_tsdtype_vec& initial_zs,
           const int windowSize,
           const aligned_tsdtype_vec& ASigmaInv,
           const aligned_tsdtype_vec& AMeanScaledSigSqrM,
           const int base_diag
           )
{
EXEC_TRACE("evaluate block of diagonals: first " << base_diag << " last " << base_diag+blocklen-1);
    const int profileLength = AMeanScaledSigSqrM.size();
	eval_diag_block_triangle(
	            result.profile.data(),
	            result.index.data(),
	            result.profile.data()+base_diag,
	            result.index.data()+base_diag,
	            initial_zs.data()+base_diag,
	            blocklen,
	            profileLength-base_diag,
	            A.data(),
	            A.data()+base_diag,
	            windowSize,
	            ASigmaInv.data(),
	            AMeanScaledSigSqrM.data(),
	            ASigmaInv.data()+base_diag,
	            AMeanScaledSigSqrM.data()+base_diag,
	            base_diag,
	            0
	    );
}

void ScrimpTrivialPar::compute_matrix_profile(const Scrimppp_params& params) {
	TIMEPOINT( tstart );
	aligned_tsdtype_vec A = fetch_time_series<aligned_tsdtype_vec::allocator_type>(params); //load the time series data
	TIMEPOINT( tfileread );
	aligned_tsdtype_vec AMean(A.size());
	aligned_tsdtype_vec ASigma(A.size());
	aligned_tsdtype_vec dotproducts(A.size());
	idx_dtype windowSize = params.query_window_len;
	idx_dtype exclusionZone = windowSize / 4;
	idx_dtype timeSeriesLength = A.size();
	idx_dtype profile_length = timeSeriesLength - windowSize + 1;
	auto result = make_aligned<MatProfSOA>();
	const int BLOCKING_SIZE = params.block_length;
	const bool distrib_io = (params.filetype == Scrimppp_params::BIN) && params.use_distributed_io;

	if (params.filetype != Scrimppp_params::BIN && params.use_distributed_io) {
		EXEC_ERROR("distributed I/O is NOT supported with ASCII files. Falling back to master I/O!");
	}

	// code instrumentaion (if enabled)
	PerfCounters ctrs_diaginit("diagonal initialization");
	PerfCounters ctrs_eval("block evaluations");
	PerfCounters ctrs_mpi_comm("MPI communication");

	//Initialize Matrix Profile and Matrix Profile Index
	result->profile.fill(-1.0);
	result->index.fill(-1);

	int world_size;
	int world_rank;
	MPI_Datatype mpi_result_type;
	MPI_Op reduction_op;
	MPI_Comm_size(MPI_COMM_WORLD, &world_size);
	MPI_Comm_rank(MPI_COMM_WORLD, &world_rank);

	if (profile_length > RESULT_ALLOC_LEN) { // check, whether the data fit into the preallocated result storage
		throw std::runtime_error("resulting profile length EXCEEDS MAXIMUM LENGTH. Recompile with different allocation size or choose a smaller problem");
	}
	else if(profile_length<=0) {
		throw std::runtime_error("parameters chosen badly: resulting matrix profile of length 0. done.");
	}

	//create the MPI dataype for the result
	const int blocklen[] = {static_cast<int>(profile_length),static_cast<int>(profile_length)};
	const MPI_Aint disps[] = {(MPI_Aint)((size_t)((result->profile).data()) - (size_t)(result.get())),
	                          (MPI_Aint)((size_t)((result->index).data()) - (size_t)(result.get())) };
	//assertion for DOUBLE usage!!! TODO employ some preprocessor or tempalte magic...
	static_assert(sizeof(tsa_dtype) == sizeof(double), "NO FLOAT POSSIBLE without modifing MPI Datatype!" );
	static_assert(std::is_same<idx_dtype, long>::value, "Mismatch between idx_dtype and respective mpi type MPI_LONG" );
	const MPI_Datatype dtypes[] = {MPI_DOUBLE, MPI_LONG};
	MPI_Type_create_struct(2, blocklen, disps, dtypes, &mpi_result_type);
	MPI_Type_commit(&mpi_result_type);

	//create the reduction operation for the custom datatype
	MPI_Op_create(MPI_MatProfSOA_reduction, 1, &reduction_op);

	// values regarding work partitioning
	const int num_partitions = 2*world_size; //number of partitions. 2 partitions per process for the sake of balancing
	const int diags_to_process_world = profile_length-exclusionZone-1; //total number of diagonals in the "adjacency" matrix to process
	// likely the partitions can not be of exactly even size. We assume the first ones to be of partition_size_full_load and the remaining ones to be one element smaller
	const PartitioningInfo partinfo = {
	    ._num_partitions = 2*world_size,
	    ._num_partitions_full_load = diags_to_process_world - ((diags_to_process_world/num_partitions) * num_partitions),
	    ._size_full_load = (diags_to_process_world/num_partitions) + 1
	};

	/*const bool proc_has_reduced_load = world_rank<num_reduced_load;
	const int diags_to_process_proc = (proc_has_reduced_load?diags_to_process_reduced_load:diags_to_process_full_load);
	const int proc_offset = (proc_has_reduced_load? world_rank*diags_to_process_reduced_load
													: diags_to_process_reduced_load*num_reduced_load
														+diags_to_process_full_load*(world_rank-num_reduced_load));*/

	// retrieve the local partitions: each process gets 2. A "long" one in the upper half of the triangle and a "short" one in the lower half of the triangle
	std::vector<DiagPartition> local_partitions;
	    // the upper partition is the "process-rank"-th one
	DiagPartition part_offsets = get_partition(world_rank, partinfo);
//NOTE: causes a integer conversion warning. It is not of interest, as using more than MAX_INT diagonals is far beyond the memory limit of a single node (currently)
	local_partitions.push_back( {exclusionZone+1+part_offsets._first_id, std::min(profile_length-1, exclusionZone+1+part_offsets._last_id)} );
	part_offsets = get_partition(num_partitions-world_rank-1, partinfo);
//NOTE: causes a integer conversion warning. It is not of interest, as using more than MAX_INT diagonals is far beyond the memory limit of a single node
	local_partitions.push_back( {exclusionZone+1+part_offsets._first_id, std::min(profile_length-1, exclusionZone+1+part_offsets._last_id)} );

	//sum up, how many diags the local proc has to evaluate
	int diags_to_process_proc = 0;
	for (auto partit = local_partitions.begin(); partit < local_partitions.end(); ++partit) {
		diags_to_process_proc += partit->_last_id - partit->_first_id +1;
	}

#ifndef NDEBUG
	// sum up over all processes
	int num_processed_diags = 0;
	MPI_Reduce(&diags_to_process_proc, &num_processed_diags, 1, MPI_INTEGER, MPI_SUM, 0, MPI_COMM_WORLD);
	if (world_rank == 0) {
		BOOST_ASSERT_MSG(num_processed_diags==diags_to_process_world, "work partitioning of the diagonals is ivalid");
	}
	EXEC_INFO("partitioning is fine!");
#endif

//	EXEC_INFO("Processing all the matrix profile!");
//	local_partitions.clear();
//	local_partitions.push_back( {exclusionZone+1, ProfileLength-1} );
/*
	const int tmprank=0;
	EXEC_INFO("Processing the rank" << tmprank << " partition of the matrix profile!");
	local_partitions.clear();
	part_offsets = get_partition(tmprank, partinfo);
	local_partitions.push_back( {exclusionZone+1+part_offsets._first_id, std::min(ProfileLength-1, exclusionZone+1+part_offsets._last_id)} );
	part_offsets = get_partition(num_partitions-tmprank-1, partinfo);
	local_partitions.push_back( {exclusionZone+1+part_offsets._first_id, std::min(ProfileLength-1, exclusionZone+1+part_offsets._last_id)} );
*/

	//validation of parameters
	if (timeSeriesLength < windowSize) {
		throw std::invalid_argument("ERROR: Time series is shorter than the window length, can not proceed");
	}

	TIMEPOINT( tsetup );

	//precompute the mean and standard deviations of the sliding windows along the time series
	precompute_window_statistics(windowSize, A, profile_length, AMean, ASigma);

	//start time measurment
	TIMEPOINT( tprecomputations );

	// randomly shuffle the comutation order of diagonal blocks (according to the blocking size).
	// In order to do so split the local partition into blocks and shuffle them.
	std::vector<DiagPartition> eval_blocks;
	eval_blocks.reserve((diags_to_process_proc/BLOCKING_SIZE) +1);

	for (auto partit = local_partitions.begin(); partit < local_partitions.end(); ++partit) {
		    // split the partitions into block which preserve cache locality
		const int partlen = partit->_last_id - partit->_first_id;
		const int num_prolonged_blocks = std::max(partlen % BLOCKING_SIZE, partlen/BLOCKING_SIZE) ;
		int prev_block_end = partit->_first_id-1;

		EXEC_TRACE("splitting into blocks a partition with start: " << partit->_first_id << " and last: " << partit->_last_id);

		for ( int blocki = 0; prev_block_end < partit->_last_id; ++blocki) {
			DiagPartition part;
			part._first_id = prev_block_end+1;
			if (blocki < num_prolonged_blocks) {
				part._last_id = std::min(part._first_id + BLOCKING_SIZE-1, partit->_last_id);
			}
			else {
				part._last_id = std::min(part._first_id+BLOCKING_SIZE-1, partit->_last_id);
			}
			eval_blocks.push_back( part );
			prev_block_end = part._last_id;

			{
				const ScopedPerfAccumulator monitor(ctrs_diaginit);
				init_diagonals(part._first_id, part._last_id, dotproducts, A, windowSize);
			}
		}
	}
	//std::random_shuffle(eval_blocks.begin(), eval_blocks.end());

	TIMEPOINT( tpartitioning )
	EXEC_DEBUG( "Proc " << world_rank << " shall process " << diags_to_process_proc << " diagonals");

	//init_all_diagonals(dotproducts, A, windowSize);

	//iteratively evaluate the diagonals of the distance matrix
	int progressctr=0;
	int next_notification_thresh = notification_interval_iter;
	for (auto blockiter = eval_blocks.begin(); blockiter < eval_blocks.end(); ++blockiter) {
		const int blocklen = blockiter->_last_id +1 - blockiter->_first_id;

		// compute distance of the first entry for each entry in the block

		    // might be more efficient, if performed with MASS for ech partition (when splitting them up into blocks)

		EXEC_DEBUG("Rank " << world_rank << " evaluating diags " << blockiter->_first_id << " to " << blockiter->_last_id);
		{
			const ScopedPerfAccumulator monitor(ctrs_eval);
			eval_diagonal_block(*result, blocklen, A, dotproducts, windowSize,  ASigma, AMean, blockiter->_first_id);
			progressctr+=blocklen;
		}
		//Show time per 10000 iterations
		if (progressctr > next_notification_thresh && false) //disable in or der for the logging not to disturb the timing
		{
			const auto tcur = get_cur_time();
			matrix_profile::Timespan time_elapsed = tcur - tpartitioning;
			EXEC_INFO ("finished " << progressctr << " iterations in: " << std::setprecision(4) << time_elapsed);
			next_notification_thresh += notification_interval_iter;
		}
	}

	TIMEPOINT( tevaluations)
	EXEC_DEBUG("apply transformation from score to distance values")
	// apply a correction of the distance values, as we dropped a factor of 2 to avoid unnecessary computations
	tsa_dtype twice_m = 2.0*static_cast<tsa_dtype>(windowSize);
	auto profile = (result->profile).data();
	for (int i = 0; i<=profile_length; ++i) {
		profile[i] = twice_m - 2.0 * profile[i];
	}
	TIMEPOINT( tpostprocessing)
	{
		const ScopedPerfAccumulator monitor(ctrs_mpi_comm);
		EXEC_DEBUG("reduce the processes individual results")
		//update matrix profile and matrix profile index if the current distance value is smaller
		if (distrib_io) { //distributed I/O => everyone needs the result => Allreduce
			// actually only a fraction is required: the partition which will be written. That would require several reductions (one for each partition), where the receiver is the process responsible for the partition...
			MPI_Allreduce(MPI_IN_PLACE, result.get(), 1, mpi_result_type, reduction_op, MPI_COMM_WORLD); // in case of distributed binary I/O everyone needs the result
		}
		else { // reduction into master only, in case it is the only one writing...
			if (world_rank != 0) {
				MPI_Reduce(result.get(), result.get(), 1, mpi_result_type, reduction_op, 0, MPI_COMM_WORLD); // Reduction of length 1, as the datatype is defined as ProfileLength!
			}
			else {
				MPI_Reduce(MPI_IN_PLACE, result.get(), 1, mpi_result_type, reduction_op, 0, MPI_COMM_WORLD); // Reduction of length 1, as the datatype is defined as ProfileLength!
			}
		}
		EXEC_DEBUG( "rank " << world_rank << " done with reduction");
	}

	TIMEPOINT( tcommunication)
	// MPI_Barrier(MPI_COMM_WORLD); //just for "debugging"

	//store the result
	if (world_rank == 0 || distrib_io ){ // storing in parallel if BIN type specified...
		EXEC_DEBUG("Rank " << world_rank << "storing the result");
		store_matrix_profile(*result, params, profile_length, distrib_io);
	}

	TIMEPOINT( tfilewrite )

	// MPI cleanup
	MPI_Op_free(&reduction_op);
	MPI_Type_free(&mpi_result_type);

	TIMEPOINT( tmpiclean)

	_timing_info.dotproduct_time = (tpartitioning-tprecomputations);
	_timing_info.setup_time = (tsetup-tfileread) + (tmpiclean-tfilewrite);
	_timing_info.comm_time = (tcommunication-tpostprocessing) ;
	_timing_info.evaluation_time = tevaluations-tpartitioning;
	_timing_info.precomp_time = tprecomputations-tsetup;
	_timing_info.comp_time = _timing_info.evaluation_time + _timing_info.dotproduct_time +_timing_info.precomp_time + (tpostprocessing-tevaluations);
	_timing_info.work_time = _timing_info.comp_time + _timing_info.comm_time + _timing_info.setup_time;
	_timing_info.io_time = (tfileread - tstart) + (tfilewrite-tcommunication);

#define STORE_TIME_TRACE( timer, description ) _timing_info._time_trace_map.insert( TracepointMap::value_type(description, timer));
	STORE_TIME_TRACE( tstart, "start");
	STORE_TIME_TRACE( tfileread, "fileread");
	STORE_TIME_TRACE( tsetup, "setup");
	STORE_TIME_TRACE( tprecomputations, "precomputations");
	STORE_TIME_TRACE( tpartitioning, "partitioning");
	STORE_TIME_TRACE( tevaluations, "matrix_eval");
	STORE_TIME_TRACE( tpostprocessing, "postprocessing");
	STORE_TIME_TRACE( tcommunication, "communication");
	STORE_TIME_TRACE( tfilewrite, "filwrite");
	STORE_TIME_TRACE( tmpiclean, "mpi_cleanup");

	ctrs_diaginit.log_perf();
	ctrs_eval.log_perf();
	ctrs_mpi_comm.log_perf();

	_log_info.profile_length = profile_length;
	_log_info.windowSize = windowSize;
	_log_info.exclusionZone =  exclusionZone;
	_log_info.timeSeriesLength = timeSeriesLength;
	_log_info.world_size = world_size;
	_log_info.world_rank = world_rank;
	_log_info.BLOCKING_SIZE = BLOCKING_SIZE;
	_log_info.diags_to_process_proc = diags_to_process_proc;
}

void ScrimpTrivialPar::store_matrix_profile(const MatProfSOA& result, const Scrimppp_params& params, const idx_dtype profileLength, const bool distributed_io)
{
	ScrimpSequ::store_matrix_profile(result.profile.data(), result.index.data(), profileLength, params, distributed_io);
}

void ScrimpTrivialPar::log_info() {
	EXEC_INFO( "MPI INFO: comm size: " << _log_info.world_size << " world rank: " << _log_info.world_rank);
	EXEC_INFO( "Blocking size: " << _log_info.BLOCKING_SIZE)
    #ifdef PROFILING
	    PERF_LOG("number of matrix profile updates: " << _profile_updates);
	    PERF_LOG("number of evaluations: " << _eval_ctr);
    #endif

	// log computation performance
	const double triang_len = _log_info.profile_length-_log_info.exclusionZone;
	auto timerprecision = std::numeric_limits<tsa_dtype>::digits10 + 2;
	PERF_LOG ( "evaluation time: " << std::setprecision(timerprecision) << _timing_info.evaluation_time);
	PERF_LOG ( "local comp time (eval+precomp): " << std::setprecision(timerprecision) << _timing_info.comp_time );
	PERF_LOG ( "communication time: " << std::setprecision(timerprecision) << _timing_info.comm_time );
	PERF_LOG ( "local working time: " << std::setprecision(timerprecision) << _timing_info.work_time );
	PERF_LOG ( "I/O time: " << std::setprecision(timerprecision) << _timing_info.io_time );
	PERF_LOG ( "dotproduct time: " << _timing_info.dotproduct_time );

	PERF_LOG ( "local throughput evaluations: " << std::setprecision(6) << _log_info.diags_to_process_proc * triang_len / get_seconds(_timing_info.evaluation_time) << " matrix entries/second" );
	PERF_LOG ( "throughput evaluations: " << std::setprecision(6) << triang_len * triang_len / get_seconds(_timing_info.evaluation_time) << " matrix entries/second (estimate based on local)" );
	PERF_LOG ( "local throughput computations: " << std::setprecision(6) << _log_info.diags_to_process_proc * triang_len / get_seconds(_timing_info.comp_time) << " matrix entries/second" );
	PERF_LOG ( "throughput computations: " << std::setprecision(6) << triang_len * triang_len / get_seconds(_timing_info.comp_time) << " matrix entries/second (estimate based on local)" );

	for (auto iter = _timing_info._time_trace_map.begin(); iter != _timing_info._time_trace_map.end(); ++iter) {
		PERF_TRACE ( "timepoint after " << iter->first << ": " << iter->second );
	}

	for (auto iter = _timing_info._synctime_map.begin(); iter != _timing_info._synctime_map.end(); ++iter) {
		PERF_LOG(" sync time " << iter->first << ": " << iter->second);
	}
}


/*
 * Copyright (c) 2013-, Stephen Miller
 * 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.
 */

#include <cpu_tsdf/octree.h>
#include <pcl/console/print.h>
#include <pcl/console/time.h>
#include <pcl/pcl_macros.h>
#include <boost/shared_ptr.hpp>
#include <vector>
#include <stdint.h>
#include <cmath>
#include <fstream>
#include <iostream>

void
cpu_tsdf::OctreeNode::getCenter (float &x, float &y, float &z) const
{
  x = ctr_x_;
  y = ctr_y_;
  z = ctr_z_;
};

void
cpu_tsdf::OctreeNode::getSize (float &size_x, float &size_y, float &size_z) const
{
  size_x = size_;
  size_y = size_;
  size_z = size_;
}

float
cpu_tsdf::OctreeNode::getMaxSize () const
{
  return (std::sqrt (3) * size_);
}

float
cpu_tsdf::OctreeNode::getMinSize () const
{
  return (size_);
}

bool
cpu_tsdf::OctreeNode::hasChildren () const
{
  return (!children_.empty ());
}

std::vector<cpu_tsdf::OctreeNode::Ptr>&
cpu_tsdf::OctreeNode::getChildren ()
{
  return children_;
}

const std::vector<cpu_tsdf::OctreeNode::Ptr>&
cpu_tsdf::OctreeNode::getChildren () const
{
  return children_;
}

void
cpu_tsdf::OctreeNode::getLeaves (std::vector<cpu_tsdf::OctreeNode::Ptr> &leaves, int num_levels)
{
  for (size_t i = 0; i < children_.size (); i++)
  {
    const OctreeNode::Ptr &child = children_[i];
    if (child->hasChildren () && num_levels != 0)
      child->getLeaves (leaves, num_levels - 1);
    else
      leaves.push_back (child);
  }
}

// Get the voxel which contains this point
cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::getContainingVoxel (float x, float y, float z, float min_size)
{
  if (!hasChildren () || (min_size > 0 && size_ <= min_size))
    return (this);
  else
  {
    return children_[((x-ctr_x_) > 0) * 4 + ((y-ctr_y_) > 0) * 2 + (z - ctr_z_ > 0)]->getContainingVoxel (x, y, z, min_size);
  }
}

// Get the voxel which contains this point
const cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::getContainingVoxel (float x, float y, float z, float min_size) const
{
  if (!hasChildren () || (min_size > 0 && size_ <= min_size))
    return (this);
  else
  {
    return children_[((x-ctr_x_) > 0) * 4 + ((y-ctr_y_) > 0) * 2 + (z - ctr_z_ > 0)]->getContainingVoxel (x, y, z, min_size);
  }
}

bool
cpu_tsdf::OctreeNode::getData (float &d, float &w) const
{
  d = d_;
  w = w_;
  return (true);
}

bool
cpu_tsdf::OctreeNode::setData (float d, float w)
{
  d_ = d;
  w_ = w;
  return (true);
}

bool
cpu_tsdf::OctreeNode::addObservation (float d_new, float w_new, float max_weight)
{
  float d_old = d_;
  d_ = (d_ * w_ + d_new * w_new) / (w_ + w_new);
  w_ += w_new;
  if (w_ > max_weight)
    w_ = max_weight;
  M_ += w_new * (d_new - d_) * (d_new - d_old);
  ++nsample_;
  return (true);
}

bool
cpu_tsdf::OctreeNode::addObservation (float d_new, float w_new, float max_weight,
                uint8_t r, uint8_t g, uint8_t b)
{
  return (addObservation (d_new, w_new, max_weight));
}

bool
cpu_tsdf::OctreeNode::getRGB (uint8_t &r, uint8_t &g, uint8_t &b) const
{
  r = g = b = 127;
  return (false);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::instantiateNode (float x, float y, float z, float sx, float sy, float sz)
{
  return (new OctreeNode (x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::OctreeNode::getTypeString ()
{
  return ("NOCOLOR");
}

cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::instantiateByTypeString (const std::string &str)
{
  if (str == "NOCOLOR")
    return (new OctreeNode);
  else if (str == "RGB")
    return (new RGBNode);
  else if (str == "RGBNormalized")
    return (new RGBNormalized);
  else if (str == "LAB")
    return (new LABNode);
  // Handle more examples
  PCL_ERROR ("[cpu_tsdf::OctreeNode::instantiateByTypeString] Requested invalid type string %s\n", str.c_str ());
  return (NULL);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::OctreeNode::instantiateByTypeString (const std::string &str,
                         float x, float y, float z, float sx, float sy, float sz)
{
  cpu_tsdf::OctreeNode* empty_node = instantiateByTypeString (str);
  cpu_tsdf::OctreeNode* node = empty_node->instantiateNode (x, y, z, sx, sy, sz);
  delete empty_node;
  return (node);
}

void
cpu_tsdf::OctreeNode::updateAverage ()
{
  if (children_.empty ())
    return;

  float d_avg = 0;
  float w_avg = 0;
  int ngood = 0;
  for (size_t i = 0; i < children_.size (); ++i)
  {
    children_[i]->updateAverage ();
    if (children_[i]->w_ > 0)
    {
      d_avg += children_[i]->d_;
      w_avg += children_[i]->w_;
      ++ngood;
    }
  }
  if (ngood > 0)
  {
    d_ = d_avg/ngood;
    w_ = w_avg/ngood;
  }
}

std::vector<cpu_tsdf::OctreeNode::Ptr>&
cpu_tsdf::OctreeNode::split ()
{
  children_.resize (8);
  //float off_x = size_x_ / 4;
  //float off_y = size_y_ / 4;
  //float off_z = size_z_ / 4;
  float off_x = size_ / 4;
  float off_y = size_ / 4;
  float off_z = size_ / 4;
  float newsize_x = size_ / 2;
  float newsize_y = size_ / 2;
  float newsize_z = size_ / 2;
  children_[0].reset (instantiateNode (ctr_x_-off_x, ctr_y_-off_y, ctr_z_-off_z, newsize_x, newsize_y, newsize_z));
  children_[1].reset (instantiateNode (ctr_x_-off_x, ctr_y_-off_y, ctr_z_+off_z, newsize_x, newsize_y, newsize_z));
  children_[2].reset (instantiateNode (ctr_x_-off_x, ctr_y_+off_y, ctr_z_-off_z, newsize_x, newsize_y, newsize_z));
  children_[3].reset (instantiateNode (ctr_x_-off_x, ctr_y_+off_y, ctr_z_+off_z, newsize_x, newsize_y, newsize_z));
  children_[4].reset (instantiateNode (ctr_x_+off_x, ctr_y_-off_y, ctr_z_-off_z, newsize_x, newsize_y, newsize_z));
  children_[5].reset (instantiateNode (ctr_x_+off_x, ctr_y_-off_y, ctr_z_+off_z, newsize_x, newsize_y, newsize_z));
  children_[6].reset (instantiateNode (ctr_x_+off_x, ctr_y_+off_y, ctr_z_-off_z, newsize_x, newsize_y, newsize_z));
  children_[7].reset (instantiateNode (ctr_x_+off_x, ctr_y_+off_y, ctr_z_+off_z, newsize_x, newsize_y, newsize_z));
  return (children_);
}

void
cpu_tsdf::OctreeNode::splitRecursive (int num_left)
{
  if (num_left <= 0)
    return;
  split ();
  for (size_t i = 0; i < children_.size (); i++)
  {
    children_[i]->splitRecursive (num_left-1);
  }
}

float
cpu_tsdf::OctreeNode::getVariance () const
{
  if (nsample_ < 5)
    return (std::numeric_limits<float>::infinity ());
  return ((M_/w_)*(nsample_/(nsample_-1)));
}

void
cpu_tsdf::OctreeNode::serialize (std::ostream &f) const
{
  f.write ((char*)&d_, sizeof (float));
  f.write ((char*)&w_, sizeof (float));
  f.write ((char*)&ctr_x_, sizeof (float));
  f.write ((char*)&ctr_y_, sizeof (float));
  f.write ((char*)&ctr_z_, sizeof (float));
  f.write ((char*)&size_, sizeof (float));
  f.write ((char*)&M_, sizeof (float));
  f.write ((char*)&nsample_, sizeof (int));
  size_t nchild = children_.size ();
  f.write ((char*)&nchild, sizeof (size_t));
  for (size_t i = 0; i < nchild; ++i)
    children_[i]->serialize (f);
}

void
cpu_tsdf::OctreeNode::deserialize (std::istream &f)
{
  f.read ((char*)&d_, sizeof (float));
  f.read ((char*)&w_, sizeof (float));
  f.read ((char*)&ctr_x_, sizeof (float));
  f.read ((char*)&ctr_y_, sizeof (float));
  f.read ((char*)&ctr_z_, sizeof (float));
  f.read ((char*)&size_, sizeof (float));
  f.read ((char*)&M_, sizeof (float));
  f.read ((char*)&nsample_, sizeof (int));
  size_t nchild;
  f.read ((char*)&nchild, sizeof (size_t));
  children_.resize (nchild);
  for (size_t i = 0; i < nchild; ++i)
  {
    children_[i].reset (instantiateNode (0, 0, 0, 0, 0, 0));
    children_[i]->deserialize (f);
  }
}

// RGBNode
bool
cpu_tsdf::RGBNode::addObservation (float d_new, float w_new, float max_weight,
                uint8_t r, uint8_t g, uint8_t b)
{
  float wsum = w_ + w_new;
  r_ = static_cast<uint8_t> ( (w_*r_ + w_new*r) / wsum);
  g_ = static_cast<uint8_t> ( (w_*g_ + w_new*g) / wsum);
  b_ = static_cast<uint8_t> ( (w_*b_ + w_new*b) / wsum);
  return (OctreeNode::addObservation (d_new, w_new, max_weight));
}

bool
cpu_tsdf::RGBNode::getRGB (uint8_t &r, uint8_t &g, uint8_t &b) const
{
  r = r_;
  g = g_;
  b = b_;
  return (true);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::RGBNode::instantiateNode (float x, float y, float z, float sx, float sy, float sz)
{
  return (new RGBNode (x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::RGBNode::getTypeString ()
{
  return ("RGB");
}

void
cpu_tsdf::RGBNode::serialize (std::ostream &f) const
{
  f.write ((char*)&r_, sizeof (uint8_t));
  f.write ((char*)&g_, sizeof (uint8_t));
  f.write ((char*)&b_, sizeof (uint8_t));
  OctreeNode::serialize (f);
}

void
cpu_tsdf::RGBNode::deserialize (std::istream &f)
{
  f.read ((char*)&r_, sizeof (uint8_t));
  f.read ((char*)&g_, sizeof (uint8_t));
  f.read ((char*)&b_, sizeof (uint8_t));
  OctreeNode::deserialize (f);
}

// RGBNormalized
bool
cpu_tsdf::RGBNormalized::addObservation (float d_new, float w_new, float max_weight,
                uint8_t r, uint8_t g, uint8_t b)
{
  float wsum = w_ + w_new;
  float i = std::sqrt ((float)r * (float)r + (float)g*(float)g + (float)b*(float)b);
  float r_f = r / i;
  float g_f = g / i;
  float b_f = b / i;
  r_n_ = (w_*r_n_ + w_new*r_f) / wsum;
  g_n_ = (w_*g_n_ + w_new*g_f) / wsum;
  b_n_ = (w_*b_n_ + w_new*b_f) / wsum;
  i_ = (w_*i_ + w_new*i) / wsum;
  return (OctreeNode::addObservation (d_new, w_new, max_weight));
}

bool
cpu_tsdf::RGBNormalized::getRGB (uint8_t &r, uint8_t &g, uint8_t &b) const
{
  r = r_n_ * i_;
  g = g_n_ * i_;
  b = b_n_ * i_;
  return (true);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::RGBNormalized::instantiateNode (float x, float y, float z, float sx, float sy, float sz)
{
  return (new RGBNormalized (x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::RGBNormalized::getTypeString ()
{
  return ("RGBNormalized");
}

void
cpu_tsdf::RGBNormalized::serialize (std::ostream &f) const
{
  f.write ((char*)&r_n_, sizeof (uint8_t));
  f.write ((char*)&g_n_, sizeof (uint8_t));
  f.write ((char*)&b_n_, sizeof (uint8_t));
  f.write ((char*)&i_, sizeof (uint8_t));
  OctreeNode::serialize (f);
}

void
cpu_tsdf::RGBNormalized::deserialize (std::istream &f)
{
  f.read ((char*)&r_n_, sizeof (uint8_t));
  f.read ((char*)&g_n_, sizeof (uint8_t));
  f.read ((char*)&b_n_, sizeof (uint8_t));
  f.read ((char*)&i_, sizeof (uint8_t));
  OctreeNode::deserialize (f);
}

void
cpu_tsdf::RGB2LAB (uint8_t r, uint8_t g, uint8_t b,
             float &L, float &A, float &B)
{
  // RGB to XYZ
  float rf = (static_cast<float> (r) / 255.);
  float gf = (static_cast<float> (g) / 255.);
  float bf = (static_cast<float> (b) / 255.);
  if (rf > 0.0405)
    rf = std::pow(((rf + 0.055) / 1.055), 2.4);
  else
    rf /= 12.92;
  if (gf > 0.0405)
    gf = std::pow(((gf + 0.055) / 1.055), 2.4);
  else
    gf /= 12.92;
  if (bf > 0.0405)
    bf = std::pow(((bf + 0.055) / 1.055), 2.4);
  else
    bf /= 12.92;
  rf *= 100;
  gf *= 100;
  bf *= 100;
  float X = rf * 0.4124 + gf * 0.3576 + bf * 0.1805;
  float Y = rf * 0.2126 + gf * 0.7152 + bf * 0.0722;
  float Z = rf * 0.0193 + gf * 0.1192 + bf * 0.9505;
  // XYZ to LAB
  X /= 95.047;
  Y /= 100.;
  Z /= 108.883;
  if (X > 0.008856)
    X = std::pow (static_cast<double> (X), 1/3.);
  else
    X = 7.787 * X + (16 / 116.);
  if (Y > 0.008856)
    Y = std::pow (static_cast<double> (Y), 1/3.);
  else
    Y = 7.787 * Y + (16 / 116.);
  if (Z > 0.008856)
    Z = std::pow (static_cast<double> (Z), 1/3.);
  else
    Z = 7.787 * Z + (16 / 116.);
  L = (116 * Y) - 16;
  A = 500 * (X - Y);
  B = 200 * (Y - Z);
}

void
cpu_tsdf::LAB2RGB (float L, float A, float B,
             uint8_t &r, uint8_t &g, uint8_t &b)
{
  // LAB to XYZ
  float Y = (L + 16) / 116.;
  float X = A / 500. + Y;
  float Z = Y - (B / 200.);
  if (std::pow (X, 3) > 0.008856)
    X = std::pow (X, 3);
  else
    X = (X - 16 / 116.) / 7.787;
  if (std::pow (Y, 3) > 0.008856)
    Y = std::pow (Y, 3);
  else
    Y = (Y - 16 / 116.) / 7.787;
  if (std::pow (Z, 3) > 0.008856)
    Z = std::pow (Z, 3);
  else
    Z = (Z - 16 / 116.) / 7.787;
  X *= 95.047;
  Y *= 100.;
  Z *= 108.883;
  // XYZ to RGB
  X /= 100;
  Y /= 100;
  Z /= 100;
  float rf = X * +3.2406 + Y * -1.5372 + Z * -0.4986;
  float gf = X * -0.9689 + Y * +1.8758 + Z * +0.0415;
  float bf = X * +0.0557 + Y * -0.2040 + Z * +1.0570;
  if (rf > 0.0031308)
    rf = 1.055 * std::pow (static_cast<double> (rf), 1. / 2.4) - 0.055;
  else
    rf *= 12.92;
  if (gf > 0.0031308)
    gf = 1.055 * std::pow (static_cast<double> (gf), 1. / 2.4) - 0.055;
  else
    gf *= 12.92;
  if (bf > 0.0031308)
    bf = 1.055 * std::pow (static_cast<double> (bf), 1. / 2.4) - 0.055;
  else
    bf *= 12.92;
  r = static_cast<uint8_t> (rf * 255);
  g = static_cast<uint8_t> (gf * 255);
  b = static_cast<uint8_t> (bf * 255);
}

// LABNode
bool
cpu_tsdf::LABNode::addObservation (float d_new, float w_new, float max_weight,
                uint8_t r, uint8_t g, uint8_t b)
{
  float wsum = w_ + w_new;
  float L_new, A_new, B_new;
  RGB2LAB (r, g, b, L_new, A_new, B_new);
  uint8_t r_reconv, g_reconv, b_reconv;
  LAB2RGB (L_new, A_new, B_new, r_reconv, g_reconv, b_reconv);
  L_ = (w_*L_ + w_new*L_new) / wsum;
  A_ = (w_*A_ + w_new*A_new) / wsum;
  B_ = (w_*B_ + w_new*B_new) / wsum;
  return (OctreeNode::addObservation (d_new, w_new, max_weight));
}

bool
cpu_tsdf::LABNode::getRGB (uint8_t &r, uint8_t &g, uint8_t &b) const
{
  LAB2RGB (L_, A_, B_, r, g, b);
  return (true);
}

cpu_tsdf::OctreeNode*
cpu_tsdf::LABNode::instantiateNode (float x, float y, float z, float sx, float sy, float sz)
{
  return (new LABNode (x, y, z, sx, sy, sz));
}

std::string
cpu_tsdf::LABNode::getTypeString ()
{
  return ("LAB");
}

void
cpu_tsdf::LABNode::serialize (std::ostream &f) const
{
  f.write ((char*)&L_, sizeof (uint8_t));
  f.write ((char*)&A_, sizeof (uint8_t));
  f.write ((char*)&B_, sizeof (uint8_t));
  OctreeNode::serialize (f);
}

void
cpu_tsdf::LABNode::deserialize (std::istream &f)
{
  f.read ((char*)&L_, sizeof (uint8_t));
  f.read ((char*)&A_, sizeof (uint8_t));
  f.read ((char*)&B_, sizeof (uint8_t));
  OctreeNode::deserialize (f);
}

// Octree
void
cpu_tsdf::Octree::init (int num_splits)
{
  // Starts with just one root node
  root_.reset (OctreeNode::instantiateByTypeString
      (voxel_type_, 0, 0, 0, size_x_, size_y_, size_z_));
  root_->splitRecursive (num_splits);
}

void
cpu_tsdf::Octree::init (float max_size_x, float max_size_y, float max_size_z)
{
  int desired_res = std::max (size_x_/max_size_x, std::max (size_y_ / max_size_y, size_z_/max_size_z));
  int num_levels = std::ceil (std::log (desired_res) / std::log (2));
  init (num_levels);
}

cpu_tsdf::OctreeNode::Ptr&
cpu_tsdf::Octree::getRoot ()
{
  return root_;
}

void
cpu_tsdf::Octree::getLeaves (std::vector<OctreeNode::Ptr> &leaves, int num_levels) const
{
  // Recursively find leaf nodes
  pcl::console::TicToc tt;
  tt.tic ();
  if (num_levels == 0)
    leaves.push_back (root_);
  else
    root_->getLeaves (leaves, num_levels-1);
}

void
cpu_tsdf::Octree::getLeaves (std::vector<OctreeNode::Ptr> &leaves, float max_size_x, float max_size_y, float max_size_z) const
{
  int desired_res = std::max (size_x_/max_size_x, std::max (size_y_ / max_size_y, size_z_/max_size_z));
  int num_levels = std::ceil (std::log (desired_res) / std::log (2));
  getLeaves (leaves, num_levels);
}

// Get the voxel which contains this point
const cpu_tsdf::OctreeNode*
cpu_tsdf::Octree::getContainingVoxel (float x, float y, float z, float min_size) const
{
  if (std::isnan (z) || std::fabs (x) > size_x_/2 || std::fabs (y) > size_y_/2 || std::fabs (z) > size_z_/2)
    return (NULL);
  return (root_->getContainingVoxel (x, y, z, min_size));
}

// Get the voxel which contains this point
cpu_tsdf::OctreeNode*
cpu_tsdf::Octree::getContainingVoxel (float x, float y, float z, float min_size)
{
  if (std::isnan (z) || std::fabs (x) > size_x_/2 || std::fabs (y) > size_y_/2 || std::fabs (z) > size_z_/2)
    return (NULL);
  return (root_->getContainingVoxel (x, y, z, min_size));
}

void
cpu_tsdf::Octree::serialize (std::ostream &f) const
{
  f << root_->getTypeString () << std::endl;
  f << "#OCTREEBINARY" << std::endl;
  f.write ((char*)&res_x_, sizeof (size_t));
  f.write ((char*)&res_y_, sizeof (size_t));
  f.write ((char*)&res_z_, sizeof (size_t));
  f.write ((char*)&size_x_, sizeof (float));
  f.write ((char*)&size_y_, sizeof (float));
  f.write ((char*)&size_z_, sizeof (float));
  root_->serialize (f);
}

void
cpu_tsdf::Octree::deserialize (std::istream &f)
{
  std::string root_type;
  f >> root_type;
  root_.reset (OctreeNode::instantiateByTypeString (root_type));
  char tmp[1024];
  do
  {
    f.getline (tmp, 1024);
  }
  while (!(tmp[0] == '#' && tmp[1] == 'O'));
  f.read ((char*)&res_x_, sizeof (size_t));
  f.read ((char*)&res_y_, sizeof (size_t));
  f.read ((char*)&res_z_, sizeof (size_t));
  f.read ((char*)&size_x_, sizeof (float));
  f.read ((char*)&size_y_, sizeof (float));
  f.read ((char*)&size_z_, sizeof (float));
  root_->deserialize (f);
}


//  Copyright (c) 2006, Giovanni P. Deretta
//
//  This code may be used under either of the following two licences:
//
//  Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
//
//  The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
//
//  THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
//  THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE. OF SUCH DAMAGE.
//
//  Or:
//
//  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 <iostream>
#include <boost/variant.hpp>
#include <boost/variant/get.hpp>
#include <boost/variant/recursive_variant.hpp>
#include <boost/variant/recursive_wrapper.hpp>
#include <boost/optional.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/mpl/begin_end.hpp>
#include <boost/mpl/deref.hpp>
#include <boost/coroutine/generator.hpp>
#include <boost/bind.hpp>
#include <boost/mpl/at.hpp>
/**
 * Solve the 'same fringe problem' using coroutines.
 * Given two binary trees, they have the same fringe
 * if all leafs, read from left to right are equals.
 * This is the classical coroutine demonstration problem,
 * because it is hard to solve in O(N) (with best case O(1))
 * without using coroutines.
 * see http://c2.com/cgi/wiki?CoRoutine
 * NOTE: this solution is an almost verbatim port of the lua solution from
 * the wiki.
 *
 * This is a cleaned up version of samefringe.cpp. It uses generators instead
 * of corutines. The variant is much more cleaner and
 * same_fringe has been simplified.
 */
namespace coroutines = boost::coroutines;
using coroutines::generator;

namespace meta {
  /**
   * Compact compile-time description of binary trees of ints.
   */
  template<typename Left, typename Right>
  class node{
    typedef Left left;
    typedef Right right;
  };

  template<int A>
  struct leaf {
    enum {value = A};
  };

  typedef
  node<node<leaf<0>, leaf<1> >, node<leaf<0>, node<leaf<5>, leaf<7> > > >
  tree_a; // fringe: 0 1 0 5 7

  typedef
  node<leaf<0>, node<leaf<1>, node<node<leaf<0>, leaf<5> >, leaf<7> > > >
  tree_b; // fringe: 0 1 0 5 7

  typedef
  node<leaf<1>, node<leaf<7>, node<node<leaf<5>, leaf<4> >, leaf<7> > > >
  tree_c; // fringe: 1 7 5 4 7
}

// This is ugly, but at least on GCC it is not fount if it is in the the global
// namespace. It could  work both in boost and in std:
// - in std   is found through ADL from pair,
// - in boost is found through ADL from pair's template argument
//   (boost::variant)
// Adding overloads in std is not portable so we fallback to the boost
// namespace.
namespace boost {
  template<typename Element>
  std::ostream& operator<<(std::ostream& out,
			   const std::pair<Element, Element>& x) {
    out << "("<< x.first << ", " << x.second<<")";
    return out;
  }
}

typedef int leaf;
typedef boost::make_recursive_variant<leaf,
				      std::pair<boost::recursive_variant_,
						boost::recursive_variant_>
>::type element;
typedef std::pair<element, element> node;

bool is_leaf(const element& x) {
  return x.which() == 0;
}

template<typename Left, typename Right>
element make_tree(meta::node<Left, Right> const&) {
  return node(make_tree(Left()),
	      make_tree(Right()));
}

template<int A>
element make_tree(meta::leaf<A> const&) {
  return leaf (A);
}

typedef generator<leaf> generator_type;

leaf
tree_leaves(generator_type::self& self, element& tree) {
  if (is_leaf(tree)) {
    self.yield(boost::get<leaf>(tree));
  } else {
    tree_leaves(self, boost::get<node>(tree).first);
    tree_leaves(self, boost::get<node>(tree).second);
  }
  self.exit();
  return 0;
}

bool same_fringe(const element& tree1, const element& tree2) {
  generator_type tree_leaves_a(boost::bind(tree_leaves, _1, tree1));
  generator_type tree_leaves_b(boost::bind(tree_leaves, _1, tree2));
  while(tree_leaves_a && tree_leaves_b) {
    if(tree_leaves_a() != tree_leaves_b())
      return false;
  }
  return true && (!tree_leaves_b && !tree_leaves_a);
}

int main() {
  std::cout << "fringe a: " << make_tree(meta::tree_a()) << "\n";
  std::cout << "fringe b: " << make_tree(meta::tree_b()) << "\n";
  std::cout << "frinte c: " << make_tree(meta::tree_c()) << "\n";
  std::cout<<"same_fringe (tree_a, tree_a) -> "<<same_fringe
    (make_tree(meta::tree_a()), make_tree(meta::tree_a()))<<"\n";
  std::cout<<"same_fringe (tree_a, tree_b) -> "<<same_fringe
    (make_tree(meta::tree_a()), make_tree(meta::tree_b()))<<"\n";
  std::cout<<"same_fringe (tree_a, tree_c) -> "<<same_fringe
    (make_tree(meta::tree_a()), make_tree(meta::tree_c()))<<"\n";
}


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

#include "esop_post_optimization.hpp"

#include <algorithm>
#include <cmath>
#include <cstdint>
#include <fstream>
#include <iostream>
#include <iterator>
#include <stack>
#include <unordered_map>
#include <vector>

#include <boost/graph/connected_components.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/pending/integer_log2.hpp>
#include <boost/range/iterator_range.hpp>

#include <core/utils/bitset_utils.hpp>
#include <core/utils/graph_utils.hpp>
#include <core/utils/range_utils.hpp>
#include <core/utils/timer.hpp>

#include <reversible/circuit.hpp>
#include <reversible/functions/add_circuit.hpp>
#include <reversible/functions/add_gates.hpp>
#include <reversible/io/print_circuit.hpp>
#include <reversible/target_tags.hpp>
#include <reversible/utils/costs.hpp>
#include <reversible/variable.hpp>

namespace cirkit
{

using edge_properties = boost::property<boost::edge_weight_t, unsigned>;
using mygraph = graph_t<boost::no_property, edge_properties>;

struct SimpleHash
{
  size_t operator()( const std::pair<unsigned, unsigned>& p ) const
  {
    return p.first ^ p.second;
  }
};

struct feature
{
  uint16_t control = 0u;
  uint16_t polarity = 0u;

  feature( gate& g );
  feature();
};

struct pair
{
  gate f;
  gate s;
  feature a;
  feature b;
  circuit const* c = nullptr;
  circuit equivalent;

  pair( const gate& f, const gate& s, const circuit& c, feature& a, feature& b );
  pair();

  bool computequivalence( unsigned& c1, unsigned& c2, double* t1, double* t2 );

  unsigned long long get_cost();

private:
  std::pair<gate, unsigned> single_control{f, 0u};
};

class optimization_graph
{
public:
  /* you can give the ESOP circuit to be optimized or directly the graph */
  optimization_graph( circuit& new_c );
  optimization_graph( const mygraph& opt_g_given );

  std::vector<edge_t<graph_t<>>> greedy_matching( bool describe );
  std::vector<edge_t<graph_t<>>> exact_matching( bool describe );
  std::vector<edge_t<graph_t<>>> realexact_matching( bool describe );

  void order_edges();

  /* this function automatically optimize for the greeding matching */
  circuit optimize_esop();

private:
  circuit c;
  mygraph opt_g;

  std::unordered_map<std::pair<unsigned, unsigned>, pair, SimpleHash> edge_to_pair;

  std::unordered_map<unsigned, std::vector<edge_t<graph_t<>>>> pos_to_match;

  std::vector<edge_t<graph_t<>>> ordered_edges;
  std::vector<edge_t<graph_t<>>> matching;
  unsigned match_w = 0;

  std::vector<vertex_t<graph_t<>>> v_saturated;
};

bool is_control( gate g, variable control )
{
  auto it = std::find( g.controls().begin(), g.controls().end(), control );
  return it != g.controls().end();
}

pair::pair( const gate& f_given, const gate& s_given, const circuit& c_given,
            feature& a_given, feature& b_given )
    : f( f_given ), s( s_given ), c( &c_given ), a( a_given ), b( b_given )
{
}

pair::pair() {}

int count( const uint16_t bits ) { return __builtin_popcount( bits ); }

int get_bit( uint16_t number, unsigned index )
{
  if ( ( ( number >> index ) & 1 ) == 1 )
    return 1;
  else
    return 0;
}

bool pair::computequivalence( unsigned& c1, unsigned& c2, double* t1,
                              double* t2 )
{
  bool combine = false;

  /* FIRST PROPERTY */
  if ( ( ( ( a.control & b.control ) & ( a.polarity ^ b.polarity ) ) == 0 ) &&
       ( ( a.control & b.control ) !=
         0 ) )
  {
    increment_timer t( t1 );
    c1++;
    auto a_notb = a.control & ( ~b.control );
    auto b_nota = ( ~a.control ) & b.control;

    if ( count( a_notb ) == 1 )
    {

      unsigned target = log2( a_notb );

      std::vector<variable> controls;
      uint16_t to_create = b_nota;
      for ( unsigned i = 0; i < 16; i++ )
      {
        if ( ( to_create & 1 ) == 1 )
        {
          if ( ( ( b.polarity & ( 1 << i ) ) >> i ) == 1 )
          {
            controls.push_back( make_var( i, true ) );
          }
          else
          {
            controls.push_back( make_var( i, false ) );
          }
        }

        to_create = to_create >> 1;
      }
      equivalent.set_lines( c->lines() );
      append_toffoli( equivalent, controls, target );
      append_toffoli( equivalent, f.controls(), f.targets().front() );
      append_toffoli( equivalent, controls, target );
      combine = true;
    }
    else if ( count( b_nota ) == 1 )
    {
      unsigned target = boost::integer_log2( b_nota );

      std::vector<variable> controls;
      uint16_t to_create = a_notb;
      for ( unsigned i = 0; i < 16; i++ )
      {
        if ( ( to_create & 1 ) == 1 )
        {
          if ( ( ( a.polarity >> i ) & 1 ) == 1 )
          {
            controls.push_back( make_var( i, true ) );
          }
          else
            controls.push_back( make_var( i, false ) );
        }
        to_create = to_create >> 1;
      }

      equivalent.set_lines( c->lines() );
      append_toffoli( equivalent, controls, target );
      append_toffoli( equivalent, s.controls(), s.targets().front() );
      append_toffoli( equivalent, controls, target );

      combine = true;
    }
  }
  /* SECOND PROPERTY */
  else if ( a.control == b.control )
  {
    increment_timer t( t2 );
    c2++;
    auto diff_pol = a.polarity ^ b.polarity;

    if ( count( diff_pol ) >= 1 )
    {
      unsigned i = 0;
      bool done = false;
      std::vector<unsigned> cnot_targets;
      unsigned cnot_control;
      std::vector<variable> common_controls;

      for ( i = 0; i < 16; ++i )
      {
        if ( ( diff_pol >> i ) & 1 )
        {
          if ( !done )
          {
            cnot_control = i;
            done = true;
            if ( ( ( a.polarity >> i ) & 1 ) == 1 )
            {
              for ( auto con : s.controls() )
              {
                if ( con.line() != i )
                {
                  common_controls.push_back( con );
                }
              }
            }
            else if ( ( ( b.polarity >> i ) & 1 ) == 1 )
            {
              for ( auto con : f.controls() )
              {
                if ( con.line() != i )
                {
                  common_controls.push_back( con );
                }
              }
            }
          }
          else
            cnot_targets.push_back( i );
        }
      }

      equivalent.set_lines( c->lines() );
      for ( auto target : cnot_targets )
      {
        append_cnot( equivalent, make_var( cnot_control, true ), target );
      }

      append_toffoli( equivalent, common_controls, s.targets().front() );

      for ( auto target : cnot_targets )
      {
        append_cnot( equivalent, make_var( cnot_control, true ), target );
      }
    }
    combine = true;
  }

  return combine;
}

unsigned long long pair::get_cost()
{
  circuit pair( c->lines() );

  append_toffoli( pair, f.controls(), f.targets().front() );
  append_toffoli( pair, s.controls(), s.targets().front() );

  return costs( pair, costs_by_gate_func( t_costs() ) );
}

bool operator==( const gate& g1, const gate& g2 )
{
  return ( g1.controls() == g2.controls() ) &&
         ( g1.targets() == g2.targets() ) && same_type( g1, g2 );
}

optimization_graph::optimization_graph( const mygraph& opt_g_given )
    : opt_g( opt_g_given )
{
}

optimization_graph::optimization_graph( circuit& new_c )
{
  double tempo = 0.0, tempo2 = 0.0, tempo3 = 0.0;
  double time_case1 = 0.0, time_case2 = 0.0;
  c = new_c;
  const auto edge_weights = boost::get( boost::edge_weight, opt_g );
  add_vertices( opt_g, new_c.num_gates() );
  std::vector<feature> stack_f;

  {
    reference_timer costa( &tempo3 );
    for ( unsigned i = 0; i < new_c.num_gates(); i++ )
    {
      feature f( new_c[i] );
      stack_f.push_back( f );
    }
  }

  auto c1 = 0u, c2 = 0u, ctr = 0u;
  /* set the loop over all the pairs of the circuit */
  {
    reference_timer costa( &tempo );
    for ( unsigned first = 0; first < new_c.num_gates() - 1; ++first )
    {
      for ( unsigned second = first + 1; second < new_c.num_gates(); ++second )
      {
        pair p( new_c[first], new_c[second], new_c, stack_f[first],
                stack_f[second] );

        bool result = false;
        ++ctr;
        result = p.computequivalence( c1, c2, &time_case1, &time_case2 );

        if ( result )
        {
          increment_timer costa2( &tempo2 );

          circuit& eq = p.equivalent;

          auto old_cost = p.get_cost();

          const auto cost = costs( eq, costs_by_gate_func( t_costs() ) );
          if ( cost < old_cost )
          {
            unsigned long long edge_gain =
                old_cost - cost; // it should be always positive

            auto e = add_edge( first, second, opt_g );
            auto edge = e.first;
            edge_weights[edge] = edge_gain;

            // map the fist source vertex with the corresponding pair
            edge_to_pair.insert( {std::make_pair( first, second ), p} );

          }
        }
      }
    }
  }
}

void optimization_graph::order_edges()
{
  auto edge_weights = boost::get( boost::edge_weight, opt_g );

  for ( auto edge : boost::make_iterator_range( boost::edges( opt_g ) ) )
  {
    ordered_edges.push_back( edge );
  }

  std::sort(
      ordered_edges.begin(), ordered_edges.end(),
      [edge_weights]( const edge_t<graph_t<>>& i, const edge_t<graph_t<>>& j ) {
        return ( edge_weights[i] > edge_weights[j] );
      } );
}

std::vector<edge_t<graph_t<>>>
optimization_graph::realexact_matching( bool describe )
{
  const auto edge_weights = boost::get( boost::edge_weight, opt_g );
  matching = {};

  std::vector<unsigned> components( boost::num_vertices( opt_g ) );
  const auto num_components =
      boost::connected_components( opt_g, &components[0] );
  if ( describe )
  {
    std::cout << "graph has" << num_components
              << "components: " << any_join( components, " " ) << std::endl;
  }

  std::vector<std::vector<unsigned>> map_connected( num_components );
  for ( unsigned i = 0; i < boost::num_vertices( opt_g ); i++ )
  {
    unsigned group = components[i];
    map_connected[group].push_back( i );
  }

  if ( describe )
  {
    unsigned conta = 0;
    for ( auto u : map_connected )
    {
      if ( describe )
      {
        std::cout << "Group " << conta << ":" << std::endl;
        for ( auto k : u )
        {
          std::cout << k << " ";
        }
        std::cout << std::endl;
      }

      conta++;
    }
  }

  // for every group >1
  for ( auto group : map_connected )
  {
    auto g_size = group.size();
    if ( g_size > 1 )
    {

      std::vector<boost::dynamic_bitset<>> match_matrix;

      std::vector<std::vector<edge_t<graph_t<>>>> matrix(
          g_size, std::vector<edge_t<graph_t<>>>( g_size ) );
      std::vector<std::vector<unsigned>> matrix_w(
          g_size, std::vector<unsigned>( g_size ) );
      std::vector<std::pair<unsigned, unsigned>>
          pair_edge; // contains all the edges in the group

      for ( auto edge : boost::make_iterator_range( boost::edges( opt_g ) ) )
      {
        auto v_source = boost::source( edge, opt_g );
        auto s_iter = std::find( group.begin(), group.end(), v_source );
        if ( s_iter != group.end() )
        {

          auto s_index = s_iter - group.begin();
          auto v_target = boost::target( edge, opt_g );
          auto t_index =
              std::find( group.begin(), group.end(), v_target ) - group.begin();
          matrix[s_index][t_index] = matrix[t_index][s_index] = edge;
          matrix_w[s_index][t_index] = matrix_w[t_index][s_index] =
              edge_weights[edge];

          pair_edge.push_back( std::make_pair( s_index, t_index ) );
        }
      }

      if ( describe )
      {
        std::cout << "matrix " << std::endl;
        for ( auto i : matrix )
        {
          for ( auto k : i )
          {
            std::cout << k << " ";
          }
          std::cout << std::endl;
        }
        std::cout << "matrix W" << std::endl;
        for ( auto i : matrix_w )
        {
          for ( auto k : i )
          {
            std::cout << k << " ";
          }
          std::cout << std::endl;
        }
        std::cout << "edges" << std::endl;
        for ( auto i : pair_edge )
        {
          std::cout << i.first << "-" << i.second << std::endl;
        }
      }

      boost::dynamic_bitset<> b( pair_edge.size() );

      unsigned wmax_combinations = 0;

      do
      {
        if ( describe )
        {
          std::cout << "combination: " << b << std::endl;
        }
        unsigned w = 0;

        std::vector<boost::dynamic_bitset<>> M(
            g_size, boost::dynamic_bitset<>( g_size ) );

        for ( unsigned bit = 0; bit < pair_edge.size(); bit++ )
        {

          if ( describe )
          {
            std::cout << "edge num: " << bit << std::endl;
          }

          unsigned source, target;
          source = pair_edge[bit].first;
          target = pair_edge[bit].second;
          if ( describe )
          {
            std::cout << source << "-->" << target << std::endl;
          }
          M[source][target] = M[target][source] = b[bit];
          if ( describe )
          {
            for ( auto row : M )
            {
              std::cout << row << std::endl;
            }
          }

          if ( b[bit] )
          {
            w += matrix_w[source][target];
          } // increment the match weigth
        }

        if ( describe )
          std::cout << "is the matrix for this combination a match?? "
                    << std::endl;
        bool match = true;
        for ( auto row : M )
        {
          if ( describe )
            std::cout << row << std::endl;
          if ( row.count() > 1 )
          {
            match = false;
          }
        }
        if ( describe )
          std::cout << match << std::endl;

        if ( match )
        {
          if ( w > wmax_combinations )
          {
            wmax_combinations = w;

            match_matrix = M;
          }
        }

        inc( b );

      } while ( b.any() );

      if ( describe )
        std::cout << "max comb weight: " << wmax_combinations << std::endl;
      if ( describe )
        std::cout << "matching matrix: ";

      unsigned count = 0u;
      for ( auto i : match_matrix )
      {
        if ( describe )
          std::cout << i << std::endl;
        for ( unsigned k = 0; k < i.size(); k++ )
        {
          if ( i[k] == 1 )
          {
            matching.push_back( matrix[count][k] );
            match_w += matrix_w[count][k];
            match_matrix[k][count] = 0;
          }
        }
        count++;
      }
    }
  }

  if ( describe )
  {
    std::cout << "the found match is: " << std::endl;
    for ( auto match_edge : matching )
    {
      std::cout << edge_weights[match_edge] << " ";
    }
    std::cout << std::endl;
    std::cout << "its weight is: " << match_w << std::endl;
  }
  return matching;
}

std::vector<edge_t<graph_t<>>>
optimization_graph::exact_matching( bool describe )
{
  const auto edge_weights = boost::get( boost::edge_weight, opt_g );

  ordered_edges = {};

  order_edges();
  if ( describe )
  {
    std::cout << "ordered edges: ";
    for ( auto edge : ordered_edges )
    {
      std::cout << edge_weights[edge] << " " << boost::source( edge, opt_g )
                << " - " << boost::target( edge, opt_g ) << ", ";
    }
    std::cout << std::endl;
  }

  unsigned max_match = 0;
  unsigned pos_match = 0;
  for ( unsigned i = 0; i < ordered_edges.size(); i++ )
  {
    matching = {};
    v_saturated = {};
    auto value_match = 0u;
    auto edge_selected = ordered_edges[i];

    if ( describe )
      std::cout << "selected is " << edge_weights[edge_selected];

    auto v_source = boost::source( edge_selected, opt_g );
    auto v_target = boost::target( edge_selected, opt_g );

    if ( describe )
      std::cout << "the vertex are " << v_source << " and " << v_target
                << std::endl;

    matching.push_back( edge_selected );
    value_match += edge_weights[edge_selected];
    // v_sat.set( v_source );
    v_saturated.push_back( v_source );
    v_saturated.push_back( v_target );

    for ( auto edge : ordered_edges )
    {
      if ( edge != edge_selected )
      {
        if ( describe )
          std::cout << "edge is " << edge_weights[edge];
        auto v_source = boost::source( edge, opt_g );
        auto v_target = boost::target( edge, opt_g );

        if ( describe )
          std::cout << "The sources are: " << v_source << "and" << v_target
                    << std::endl;

        if ( std::find( v_saturated.begin(), v_saturated.end(), v_source ) ==
                 v_saturated.end() &&
             std::find( v_saturated.begin(), v_saturated.end(), v_target ) ==
                 v_saturated.end() )
        {

          if ( describe )
            std::cout << "can be in the matching" << std::endl;

          matching.push_back( edge );
          value_match += edge_weights[edge];
          v_saturated.push_back( v_source );
          v_saturated.push_back( v_target );
        }
        else
        {
          if ( describe )
            std::cout << "cannot be in the matching" << std::endl;
        }
      }
    }

    pos_to_match[i] = matching;

    if ( value_match > max_match )
    {
      max_match = value_match;
      pos_match = i;
    }
  }

  matching = pos_to_match[pos_match];
  if ( describe )
  {
    std::cout << "the found match is: " << std::endl;
    for ( auto match_edge : matching )
    {
      std::cout << edge_weights[match_edge] << " ";
    }
    std::cout << std::endl;
  }
  return matching;
}

std::vector<edge_t<graph_t<>>>
optimization_graph::greedy_matching( bool describe )
{

  const auto edge_weights = boost::get( boost::edge_weight, opt_g );

  order_edges();

  for ( auto edge : ordered_edges )
  {

    auto v_source = boost::source( edge, opt_g );
    auto v_target = boost::target( edge, opt_g );

    if ( describe )
      std::cout << "The sources are: " << v_source << "and" << v_target
                << std::endl;

    if ( std::find( v_saturated.begin(), v_saturated.end(), v_source ) ==
             v_saturated.end() &&
         std::find( v_saturated.begin(), v_saturated.end(), v_target ) ==
             v_saturated.end() )
    {
      if ( describe )
        std::cout << "can be in the matching" << std::endl;
      matching.push_back( edge );

      v_saturated.push_back( v_source );
      v_saturated.push_back( v_target );
    }
    else
    {
      if ( describe )
        std::cout << "cant be in the matching" << std::endl;
    }
  }
  if ( describe )
  {
    std::cout << "the found match is: " << std::endl;
    for ( auto match_edge : matching )
    {
      std::cout << edge_weights[match_edge] << " ";
    }
    std::cout << std::endl;
  }

  return matching;
}

circuit optimization_graph::optimize_esop()
{
  std::vector<unsigned> untouched_pos;

  for ( unsigned pos = 0u; pos < c.num_gates(); ++pos )
  {
    untouched_pos.push_back( pos );
  }

  circuit c_optimized( c.lines() );
  for ( auto edge : matching )
  {

    auto v_source = boost::source( edge, opt_g );
    auto v_target = boost::target( edge, opt_g );
    std::pair<unsigned, unsigned> key = std::make_pair( v_source, v_target );
    pair& p_comb = edge_to_pair[key];

    for ( const auto& g : p_comb.equivalent )
    {
      c_optimized.append_gate() = g;
    }
    untouched_pos.erase(
        std::remove( untouched_pos.begin(), untouched_pos.end(), v_source ),
        untouched_pos.end() );
    untouched_pos.erase(
        std::remove( untouched_pos.begin(), untouched_pos.end(), v_target ),
        untouched_pos.end() );
  }

  for ( unsigned pos : untouched_pos )
  {
    append_toffoli( c_optimized, c[pos].controls(), c[pos].targets().front() );
  }

  return c_optimized;
}
feature::feature() {}

feature::feature( gate& g )
{

  for ( auto c : g.controls() )
  {
    control |= ( 1 << c.line() );
    if ( c.polarity() == 1 )
    {
      polarity |= ( 1 << c.line() );
    }
  }
}

circuit esop_post_optimization( const circuit& c, const properties::ptr& settings,
                                const properties::ptr& statistics )
{
  const auto verbose = get( settings, "verbose", false );
  const auto greedy = get( settings, "greedy", true );
  const auto exact = get( settings, "exact", false );

  if ( greedy && exact )
  {
    std::cout << "[w] both optimization approaches are enabled, will perform greedy matching" << std::endl;
  }

  properties_timer t( statistics );
  double* tempo;

  circuit new_c = c;

  optimization_graph g( new_c );
  std::vector<edge_t<graph_t<>>> match;

  if ( greedy )
  {
    match = g.greedy_matching( false );
  }
  else if ( exact )
  {
    match = g.realexact_matching( false );
  }

  if ( verbose )
  {
    std::cout << c << std::endl;
  }

  new_c = g.optimize_esop();

  if ( verbose )
  {
    std::cout << new_c << std::endl;
  }

  unsigned global_pre = costs( c, costs_by_gate_func( t_costs() ) );
  unsigned global_post = costs( new_c, costs_by_gate_func( t_costs() ) );

  unsigned global_cost_gain = global_pre - global_post;

  double percentage = global_cost_gain;
  percentage /= global_pre;

  double match_percentage = match.size();
  match_percentage /= c.num_gates();

  set( statistics, "% matched pairs", match_percentage );
  set( statistics, "global improvement", percentage );

  return new_c;
}
}

// Local Variables:
// c-basic-offset: 2
// eval: (c-set-offset 'substatement-open 0)
// eval: (c-set-offset 'innamespace 0)
// End:


#ifndef BOOST_NUMERIC_CPP_HPP
#define BOOST_NUMERIC_CPP_HPP

// MS compatible compilers support #pragma once
#if defined(_MSC_VER) && (_MSC_VER >= 1020)
# pragma once
#endif

//  Copyright (c) 2012 Robert Ramey
//
// 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)

// policy which creates results types equal to that of C++ promotions.
// Using the policy will permit the program to build and run in release
// mode which is identical to that in debug mode except for the fact
// that errors aren't trapped.

#include <type_traits> // integral constant, remove_cv, conditional
#include <limits>
#include <boost/integer.hpp> // integer type selection

#include "safe_common.hpp"
#include "checked_result.hpp"

namespace boost {
namespace safe_numerics {

// in C++ the following rules govern integer arithmetic

// This policy is use to emulate another compiler/machine architecture
// For example, a Z80 has 8 bit char, 16 bit short, 16 bit int, 32 bit long.  So one
// would use cpp<8, 16, 16, 32, 32> to test programs destined to run on a Z80

// Follow section 5 of the standard.
template<
    int CharBits,
    int ShortBits,
    int IntBits,
    int LongBits,
    int LongLongBits
>
struct cpp {
public:
    using local_char_type = typename boost::int_t<CharBits>::exact;
    using local_short_type = typename boost::int_t<ShortBits>::exact;
    using local_int_type = typename boost::int_t<IntBits>::exact;
    using local_long_type = typename boost::int_t<LongBits>::exact;
    using local_long_long_type = typename boost::int_t<LongLongBits>::exact;

    template<class T>
    using rank =
        typename std::conditional<
            std::is_same<local_char_type, typename std::make_signed<T>::type>::value,
            std::integral_constant<int, 1>,
        typename std::conditional<
            std::is_same<local_short_type, typename std::make_signed<T>::type>::value,
            std::integral_constant<int, 2>,
        typename std::conditional<
            std::is_same<local_int_type, typename std::make_signed<T>::type>::value,
            std::integral_constant<int, 3>,
        typename std::conditional<
            std::is_same<local_long_type, typename std::make_signed<T>::type>::value,
            std::integral_constant<int, 4>,
        typename std::conditional<
            std::is_same<local_long_long_type, typename std::make_signed<T>::type>::value,
            std::integral_constant<int, 5>,
            std::integral_constant<int, 6> // catch all - never promote integral
        >::type >::type >::type >::type >::type;

    // section 4.5 integral promotions

   // convert smaller of two types to the size of the larger
    template<class T, class U>
    using higher_ranked_type = typename std::conditional<
        (rank<T>::value < rank<U>::value),
        U,
        T
    >::type;

    template<class T, class U>
    using copy_sign = typename std::conditional<
        std::is_signed<U>::value,
        typename std::make_signed<T>::type,
        typename std::make_unsigned<T>::type
    >::type;

    template<class T>
    using integral_promotion = copy_sign<
        higher_ranked_type<local_int_type, T>,
        T
    >;

    // note presumption that T & U don't have he same sign
    // if that's not true, these won't work
    template<class T, class U>
    using select_signed = typename std::conditional<
        std::numeric_limits<T>::is_signed,
        T,
        U
    >::type;

    template<class T, class U>
    using select_unsigned = typename std::conditional<
        std::numeric_limits<T>::is_signed,
        U,
        T
    >::type;

    // section 5 clause 11 - usual arithmetic conversions
    template<typename T, typename U>
    using usual_arithmetic_conversions =
        // clause 0 - if both operands have the same type
        typename std::conditional<
            std::is_same<T, U>::value,
            // no further conversion is needed
            T,
        // clause 1 - otherwise if both operands have the same sign
        typename std::conditional<
            std::numeric_limits<T>::is_signed
            == std::numeric_limits<U>::is_signed,
            // convert to the higher ranked type
            higher_ranked_type<T, U>,
        // clause 2 - otherwise if the rank of he unsigned type exceeds
        // the rank of the of the signed type
        typename std::conditional<
            rank<select_unsigned<T, U>>::value
            >= rank< select_signed<T, U>>::value,
            // use unsigned type
            select_unsigned<T, U>,
        // clause 3 - otherwise if the type of the signed integer type can
        // represent all the values of the unsigned type
        typename std::conditional<
            std::numeric_limits< select_signed<T, U>>::digits >=
            std::numeric_limits< select_unsigned<T, U>>::digits,
            // use signed type
            select_signed<T, U>,
        // clause 4 - otherwise use unsigned version of the signed type
            std::make_signed< select_signed<T, U>>
        >::type >::type >::type
    >;

    template<typename T, typename U>
    using result_type = typename usual_arithmetic_conversions<
        integral_promotion<typename base_type<T>::type>,
        integral_promotion<typename base_type<U>::type>
    >::type;
public:
    template<typename T, typename U>
    struct addition_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct subtraction_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct multiplication_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct division_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct modulus_result {
       using type = result_type<T, U>;
    };
    // note: comparison_result (<, >, ...) is special.
    // The return value is always a bool.  The type returned here is
    // the intermediate type applied to make the values comparable.
    template<typename T, typename U>
    struct comparison_result {
        using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct left_shift_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct right_shift_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct bitwise_and_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct bitwise_or_result {
       using type = result_type<T, U>;
    };
    template<typename T, typename U>
    struct bitwise_xor_result {
       using type = result_type<T, U>;
    };
};

} // safe_numerics
} // boost

#endif // BOOST_NUMERIC_cpp_HPP


/////////////////////////////////////////////////////////////
//                                                         //
// Copyright (c) 2003-2017 by The University of Queensland //
// Centre for Geoscience Computing                         //
// http://earth.uq.edu.au/centre-geoscience-computing      //
//                                                         //
// Primary Business: Brisbane, Queensland, Australia       //
// Licensed under the Open Software License version 3.0    //
// http://www.apache.org/licenses/LICENSE-2.0              //
//                                                         //
/////////////////////////////////////////////////////////////

#include <boost/version.hpp>
#include <boost/python.hpp>
#include "Python/esys/lsm/geometry/MiscPy.h"
#include "Python/esys/lsm/geometry/SimpleSpherePy.h"
#include "Python/esys/lsm/util/BoundingBoxPy.h"
#include "Python/esys/lsm/util/Vec3Py.h"

namespace esys
{
  namespace lsm
  {
    class SolidBoxPy : public BoundingBoxPy
    {
    public:
      SolidBoxPy() : BoundingBoxPy()
      {
      }

      SolidBoxPy(const Vec3Py &minPt, const Vec3Py &maxPt)
        : BoundingBoxPy(minPt, maxPt)
      {
      }

      SolidBoxPy(const SolidBoxPy &box)
        : BoundingBoxPy(box.getMinPt(), box.getMaxPt())
      {
      }

      SolidBoxPy(const BoundingBoxPy &box)
        : BoundingBoxPy(box.getMinPt(), box.getMaxPt())
      {
      }

      SolidBoxPy(const BoundingBox &box)
        : BoundingBoxPy(box.getMinPt(), box.getMaxPt())
      {
      }

      /**
       * Arvo's algorithm for axis-aligned solid-box/solid-sphere intersection.
       */
      bool intersectsWithSphere(const Vec3 &centre, double radius) const
      {
        double distSqrd = 0.0;
        for (int i = 0; i < 3; i++)
        {
          if (centre[i] < getMinPt()[i])
          {
            const double diff = centre[i] - getMinPt()[i];
            distSqrd += diff*diff;
          }
          else if (centre[i] > getMaxPt()[i])
          {
            const double diff = centre[i] - getMaxPt()[i];
            distSqrd += diff*diff;
          }
        }
        return (distSqrd <= (radius*radius));
      }

      bool intersectsWithSpherePy(const Vec3Py &centre, double radius) const
      {
        return intersectsWithSphere(centre, radius);
      }

      bool intersects(const SimpleSpherePy &p) const
      {
        return intersectsWithSphere(p.getPos(), p.getRad());
      }
    };

    class HollowBoxPy : public BoundingBoxPy
    {
    public:
      HollowBoxPy() : BoundingBoxPy()
      {
      }

      HollowBoxPy(const Vec3Py &minPt, const Vec3Py &maxPt)
        : BoundingBoxPy(minPt, maxPt)
      {
      }

      HollowBoxPy(const HollowBoxPy &box)
        : BoundingBoxPy(box.getMinPt(), box.getMaxPt())
      {
      }

      HollowBoxPy(const BoundingBoxPy &box)
        : BoundingBoxPy(box.getMinPt(), box.getMaxPt())
      {
      }

      HollowBoxPy(const BoundingBox &box)
        : BoundingBoxPy(box.getMinPt(), box.getMaxPt())
      {
      }

      /**
       * Arvo's algorithm for axis-aligned hollow-box/solid-sphere intersection.
       */
      bool intersectsWithSphere(const Vec3 &centre, double radius) const
      {
        double distSqrd = 0;
        bool face = false;
        for(int i = 0; i < 3; i++)
        {
          if(centre[i] < getMinPt()[i])
          {
            face = true;
            const double diff = (centre[i] - getMinPt()[i]);
            distSqrd += diff*diff;
          }
          else if (centre[i] > getMaxPt()[i])
          {
            face = true;
            const double diff = centre[i] - getMaxPt()[i];
            distSqrd += diff*diff;
          }
          else if ((centre[i] - getMinPt()[i]) <= radius )
          {
            face = true;
          }
          else if ((getMaxPt()[i] - centre[i]) <= radius)
          {
            face = true;
          }
        }
        return (face && (distSqrd <= radius*radius));
      }

      bool intersectsWithSpherePy(const Vec3Py &centre, double radius) const
      {
        return intersectsWithSphere(centre, radius);
      }

      bool intersects(const SimpleSpherePy &p) const
      {
        return intersectsWithSphere(p.getPos(), p.getRad());
      }
    };

    void exportMisc()
    {
      // Disable autogeneration of C++ signatures (Boost 1.34.0 and higher)
      // for Epydoc which stumbles over indentation in the automatically generated strings.
      boost::python::docstring_options no_autogen(true,false);

      boost::python::class_<SolidBoxPy, boost::python::bases<BoundingBoxPy> >(
        "SolidBox"
      )
      .def(boost::python::init<>())
      .def(boost::python::init<const Vec3Py &, const Vec3Py &>())
      .def(boost::python::init<const SolidBoxPy &>())
      .def(boost::python::init<const BoundingBoxPy &>())
      .def("intersects", &SolidBoxPy::intersects)
      .def("intersects", &SolidBoxPy::intersectsWithSpherePy)
      ;

      boost::python::class_<HollowBoxPy, boost::python::bases<BoundingBoxPy> >(
        "HollowBox"
      )
      .def(boost::python::init<>())
      .def(boost::python::init<const Vec3Py &, const Vec3Py &>())
      .def(boost::python::init<const HollowBoxPy &>())
      .def(boost::python::init<const BoundingBoxPy &>())
      .def("intersects", &HollowBoxPy::intersects)
      .def("intersects", &HollowBoxPy::intersectsWithSpherePy)
      ;
    }
  }
}


/*
 * Copyright 2019 © Centre Interdisciplinaire de développement en Cartographie des Océans (CIDCO), Tous droits réservés
 */

#ifndef SOUNDVELOCITYPROFILE_HPP
#define SOUNDVELOCITYPROFILE_HPP

#include <list>
#include <Eigen/Dense>
#include <iostream>
#include <fstream>
#include <sstream>
#include <cmath>
#include <vector>
#include <ctime>
#include <string>
#include "../utils/TimeUtils.hpp"

/*!
 * \brief SoundVelocityProfile class
 * \author Guillaume Labbe-Morissette, Jordan McManus, Emile Gagne
 * \date August 22, 2018, 10:30 AM
 */
class SoundVelocityProfile {
public:

    /**Creates a sound velocity*/
    SoundVelocityProfile() {
        longitude = latitude = nan("");
        timestamp = 0;
    }

    /**Destroys a sound velocity*/
    ~SoundVelocityProfile() {

    }

    /**Returns the size of the SoundVelocityProfile*/
    unsigned int getSize() {
        return getDepths().size();
    };

    /**Returns the latitude of the SoundVelocityProfile*/
    double getLatitude() {
        return latitude;
    }

    /**
     * Sets the latitude of the SoundVelocityProfile
     *
     * @param l the new latitude
     */
    void setLatitude(double l) {
        latitude = l;
    }

    /**Returns the longitude of the SoundVelocityProfile*/
    double getLongitude() {
        return longitude;
    }

    /**
     * Sets the longitude of the SoundVelocityProfile
     *
     * @param l the new longitude of the SoundVelocityProfile
     */
    void setLongitude(double l) {
        longitude = l;
    }

    /**Return the timestamp of the sound velocity*/
    uint64_t getTimestamp() {
        return timestamp;
    };

    /**
     * Sets the timestamp of the SoundVelocityProfile
     *
     * @param t the new timestamp
     */
    void setTimestamp(uint64_t t) {
        timestamp = t;
    };

    /**
     * Adds a new value in the vector depths and speeds
     *
     * @param depth value to add in depths
     * @param soundSpeed value to add in speeds
     */
    void add(double depth, double soundSpeed) {
        samples.push_back(std::make_pair(depth, soundSpeed));
    }

    /**Returns the depths vector*/
    Eigen::VectorXd & getDepths() {
        //lazy load internal vector
        if ((unsigned int) depths.size() != samples.size()) {
            depths.resize(samples.size());

            for (unsigned int i = 0; i < samples.size(); i++) {
                depths(i) = samples[i].first;
            }
        }

        return depths;
    }

    /**Returns the speeds vector*/
    Eigen::VectorXd & getSpeeds() {
        //lazy load internal vector
        if ((unsigned int) speeds.size() != samples.size()) {
            speeds.resize(samples.size());

            for (unsigned int i = 0; i < samples.size(); i++) {
                speeds(i) = samples[i].second;
            }
        }

        return speeds;
    }

    /**Returns the sound speed gradient*/
    Eigen::VectorXd & getSoundSpeedGradient() {
        if( (unsigned int) gradient.size() != samples.size()-1) {
            gradient.resize(samples.size() - 1);

            for (unsigned int k=0; k < samples.size()-1; k++){
                gradient(k) = (getSpeeds()(k+1)- getSpeeds()(k))/(getDepths()(k+1)- getDepths()(k));
            }
        }

        return gradient;
    }

    /**Returns the layer index at specified depth*/
    unsigned int getLayerIndexForDepth(double depth) {
        // Warning: do not confuse layer with svp sample
        // a layer is the space above or beneath a sample

        if(depth < samples[0].first) {
            return 0; // the 0-th layer is shallower than the shallowest svp sample
        }

        for (unsigned int k=0; k < samples.size()-1; k++){
            if( depth >= samples[k].first && depth < samples[k+1].first) {
                return k+1;
            }
        }

        // This layer is deeper than the deepest svp sample
        return samples.size();
    }

    /**
     * Returns the stream in which this ping will be written
     *
     * @param os the stream in which to write this ping
     * @param obj the ping to write in the stream
     */
    friend std::ostream& operator<<(std::ostream & os, const SoundVelocityProfile & obj) {
        return os <<
                "timestamp: " << obj.timestamp << std::endl <<
                "latitude: " << obj.latitude << std::endl <<
                "longitude: " << obj.longitude << std::endl;
    }

private:

    /**timestamp value of the SoundVelocityProfile (micro-second)*/
    uint64_t timestamp; //timestamp

    /**latitude value of the SoundVelocityProfile*/
    double latitude;

    /**longitude value of the SoundVelocityProfile*/
    double longitude;

    /**vector that contains the dephts of the SoundVelocityProfile*/
    Eigen::VectorXd depths;

    /**vector that contain the speeds of the SoundVelocityProfile*/
    Eigen::VectorXd speeds;

    /**vector that contain sound speed gradient*/
    Eigen::VectorXd gradient;

    /**vector that contain the depths and the speeds*/
    std::vector<std::pair<double, double>> samples;
};

#endif /* SOUNDVELOCITYPROFILE_HPP */


/// @file gibbs.hpp A generic templated Gibbs sampler implementation

#ifndef SAMPLING_GIBBS_HPP__
#define SAMPLING_GIBBS_HPP__

#include <boost/tuple/tuple.hpp>
#include <functional>
#include <stdexcept>

#include "../partition_sampler.hpp"

namespace biggles {

typedef boost::tuple<partition_sampler_sample, model::parameters> tracker_sample;

namespace sampling {

/// @brief An implementation of a Gibbs sampler.
///
/// A Gibbs sampler can sample from some conditional joint posterior distribution, \f$ P(x_1, x_2, ..., x_N | Y) \f$
/// given only the ability to sample from individual conditional probability distributions \f$ P(x_i | x_1, ...,
/// x_{i-1}, x_{i+1}, ..., x_N, Y) \f$.
///
/// Each sample from the sampler is a tuple \f$(x_1, x_2, ..., x_N)\f$ of samples from the joint distribution. This
/// tuple has type \p SampleTuple which must be an appropriate form of \c boost::tuple<>. The sampler maintains a copy
/// of the last sample tuple drawn which may be accessed via the last_sample() member function.
///
/// The sampler draws samples from the individual conditional distributions via the \p ConditionalSampler type. The
/// sampler maintains an internal instance of this type which is used to draw samples. If \c cs is an instance of \p
/// ConditionalSampler and \c st is an instance of \c SampleTuple, then the following expression should draw a sample
/// for \f$x_{i+1}\f$ where \f$0 \le i < N\f$:
///
/// @code
/// cs.template draw<i>(st);
/// @endcode
///
/// The sampler may use values within \c st not corresponding to index \c i as conditional values for sampling. As a
/// concrete example, the following \c struct would be a minimal declaration matching the \c ConditionalSampler concept.
///
/// @code
/// #include <boost/tuple.hpp>
///
/// // ...
///
/// template<typename SampleTuple>
/// struct ConditionalSampler
/// {
///     // Draw a sample for x_{idx+1} from P(x_{idx+1} | x_1, ..., x_{idx}, x_{idx+2}, ..., x_N).
///     template<int idx>
///     boost::tuples::element<idx, SampleTuple>::type draw(const SampleTuple&);
/// };
/// @endcode
///
/// @sa two_var_cond_sampler An example of a \p ConditionalSampler which assumes that the Gibbs sampler is sampling from
/// the joint distribution of just two random variables.
///
/// @tparam SampleTuple A boost::tuple which specifies the types of \f$ x_1 \f$, \f$ x_2 \f$, etc.
/// @tparam ConditionalSampler A sampler which can sample from each conditional distribution. See
/// two_var_cond_sampler for an example.
template<typename ConditionalSampler, typename SampleTuple = typename ConditionalSampler::sample_tuple_type>
class gibbs_sampler
{
public:
    /// @brief Specify initial sample tuple and conditional sampler instance.
    ///
    /// @param cond_sampler A copy of this conditional sampler is taken to initialise the internal instance.
    /// @param initial_sample The initial sample tuple to use for the sampler.
    gibbs_sampler(const ConditionalSampler& cond_sampler = ConditionalSampler(),
            const SampleTuple& initial_sample = SampleTuple())
        : sample_(initial_sample)
        , cond_sampler_(cond_sampler)
    { }

    /// @brief Draw a new sample from the conditional joint distribution.
    ///
    /// @return A reference to the sample drawn by the sampler which can be queried before the next invocation of draw()
    /// via the last_sample() member function.
    const SampleTuple& draw()
    {
        typedef gibbs_sampler<ConditionalSampler, SampleTuple> self_t;

        // start the sample drawing process.
        sample_drawer<self_t, boost::tuples::length<SampleTuple>::value, 0>()(*this);
        return last_sample();
    }

    /// @brief Obtain a reference to the last sample drawn by the sampler.
    const SampleTuple& last_sample() const { return sample_; }

protected:
    /// @brief The current sample tuple for the sampler.
    SampleTuple sample_;

    /// @brief The sample evolution functor.
    ConditionalSampler cond_sampler_;

private:
    /// These implement a template metaprogramming recursion to draw one sample in turn from the conditional PDF.
    /// @{

    template<typename Parent, int maxidx, int idx>
    struct sample_drawer
    {
        inline void operator() (Parent& p) const
        {
            boost::tuples::get<idx>(p.sample_) = p.cond_sampler_.template draw<idx>(p.sample_);
            sample_drawer<Parent, maxidx, idx+1>()(p);
        };
    };

    template<typename Parent, int maxidx>
    struct sample_drawer<Parent, maxidx, maxidx>
    {
        inline void operator() (Parent&) const { }
    };

    /// @}
};

class new_gibbs_sampler {
    //new_gibbs_sampler();
    new_gibbs_sampler(const new_gibbs_sampler&);
    void operator=(const new_gibbs_sampler&);
public:
    new_gibbs_sampler(const partition_ptr_t& initial_partition_ptr = partition_ptr_t(new partition),
        const model::parameters& initial_parameters = model::parameters());
    tracker_sample draw();
    tracker_sample last_sample() const;
    const partition_sampler_sample& last_partition_sample() const;
    const model::parameters& last_parameter_sample() const;
    /** set the observation error to the passed values and stop sampling it
     *
     */
    void fix_observation_error(const float R00, const float R01, const float R11);
    /** \brief draw a sample of the oservation error every *lag* samples
     *
     * lag==0 does the same as lag==1
     */
    void sample_observation_error(const size_t lag = 1);
protected:
    /// \brief read access to the Metropolis-Hastings sampler
    const partition_sampler& underlying_partition_sampler() const { return partition_sampler_; }
    /// \brief write access to the Metropolis-Hastings sampler
    partition_sampler& underlying_partition_sampler() { return partition_sampler_; }
private:
    size_t observation_error_lag_; ///< \brief the gap between two observation error samples
    size_t gibbs_sample_count_; ///< \brief the internal counter for the observation error samples
    partition_sampler_sample partition_sample_; ///< \brief the current partition sample incl. proposed/executed moves
    model::parameters parameter_sample_; ///< \brief the current parameter sample
    partition_sampler partition_sampler_; ///< \brief the Metropolis-Hastings sampler
};

namespace gibbs {

/// @brief An base class for a two-variable sampler.
///
/// This class serves as an implementation of the \c ConditionalSampler concept which can sample from distributions of
/// the form \f$ P(x_1, x_2 | Y) \f$. It does this by taking two \c UnaryFunction functors which can draw a sample of
/// \f$ x_1 \f$ given \f$ x_2 \f$ and vice-versa. Any static data, \f$Y\f$, should be hidden within the functor state.
///
/// The sampler is templated on two types implementing the \c UnaryFunction concept, \p UnaryFunction1 and \p
/// UnaryFunction2. The return type of \p UnaryFunction1 should correspond to \f$ x_1 \f$ and the return type of \p
/// UnaryFunction2 should correspond to \f$ x_2 \f$. The argument types should correspond to \f$ x_2 \f$ and \f$ x_1 \f$
/// respectively.
///
/// The instances of these functors provided should sample values of \f$ x_1 \f$ and \f$ x_2 \f$ appropriately
/// conditioned on their arguments.
///
/// @sa mem_fun_two_var_cond_sampler A convenience derived class capable of using its member functions as sampling
/// functions.
///
/// @tparam UnaryFunction1 A std::unary_function to draw samples of \f$x_1\f$.
/// @tparam UnaryFunction2 A std::unary_function to draw samples of \f$x_2\f$.
template<typename UnaryFunction1, typename UnaryFunction2>
class two_var_cond_sampler
{
public:
    /// @brief The C++ type corresponding to \f$ x_1 \f$.
    typedef typename UnaryFunction1::result_type first_sample_type;

    /// @brief The C++ type corresponding to \f$ x_2 \f$.
    typedef typename UnaryFunction2::result_type second_sample_type;

    /// @brief The C++ type corresponding to the tuple \f$ (x_1, x_2) \f$.
    typedef boost::tuple<first_sample_type, second_sample_type> sample_tuple_type;

    /// @brief Initialise the sampling functors.
    ///
    /// @param first_sampler An instance of \p UnaryFunction1 which is copied internally.
    /// @param second_sampler An instance of \p UnaryFunction2 which is copied internally.
    two_var_cond_sampler(
        const UnaryFunction1 first_sampler = UnaryFunction1(),
        const UnaryFunction2 second_sampler = UnaryFunction2())
        :   first_sampler_(first_sampler)
        ,   second_sampler_(second_sampler)
    { }

    /// @brief Draw a sample from one conditional distribution.
    ///
    /// Draw a sample of \f$ x_{i+1} \f$ from the individual conditional distributions.
    ///
    /// @tparam i Which random variable to sample.
    /// @param sample_tuple A tuple giving the conditional values.
    ///
    /// @return A new sample of \f$ x_{i+1} \f$.
    template<int i>
    inline typename boost::tuples::element<i, sample_tuple_type>::type draw(const sample_tuple_type& sample_tuple)
    {
        typedef two_var_cond_sampler<UnaryFunction1, UnaryFunction2> self_t;
        return sampler_func<self_t, i>()(*this, boost::tuples::get<1-i>(sample_tuple));
    }

protected:
    /// @brief A functor for sampling \f$x_1\f$ from \f$P(x_1 | x_2, Y)\f$.
    UnaryFunction1 first_sampler_;

    /// @brief A functor for sampling \f$x_2\f$ from \f$P(x_2 | x_1, Y)\f$.
    UnaryFunction2 second_sampler_;

private:
    /// These implement a templated functor for sampling from either the first or second sampler.
    /// @{

    template<typename Parent, int>
    struct sampler_func { };

    template<typename Parent>
    struct sampler_func<Parent, 0>
    {
        inline first_sample_type operator() (Parent& p, const second_sample_type& a) const
        {
            return p.first_sampler_(a);
        }
    };

    template<typename Parent>
    struct sampler_func<Parent, 1>
    {
        inline second_sample_type operator() (Parent& p, const first_sample_type& a) const
        {
            return p.second_sampler_(a);
        }
    };

    /// @}
};

/// @brief An abstract base class for a class which implements two-variable conditional sampling in member functions.
///
/// Derived classes should implement the following the two member functions \c sample_variable_1() and \c
/// sample_variable_2(). For example:
///
/// @code
/// template<typename Variable1, typename Variable2>
/// struct derived_two_var_cond_sampler
///     : public mem_fun_two_var_cond_sampler<derived_two_var_cond_sampler, Variable1, Variable2>
/// {
///     Variable1 sample_variable_1(Variable2);
///
///     Variable2 sample_variable_2(Variable1);
/// };
/// @endcode
///
/// @tparam Derived The type of the derived class.
/// @tparam Variable1 The type corresponding to \f$ x_1 \f$.
/// @tparam Variable2 The type corresponding to \f$ x_2 \f$.
template<typename Derived, typename Variable1, typename Variable2>
class mem_fun_two_var_cond_sampler
    : public two_var_cond_sampler<
      std::binder1st<std::mem_fun1_t<Variable1, Derived, Variable2> >,
      std::binder1st<std::mem_fun1_t<Variable2, Derived, Variable1> > >
{
public:
    /// @brief A \c UnaryFunctor type which returns a sample of \f$ x_1 \f$ given a value of \f$ x_2 \f$.
    typedef std::binder1st<std::mem_fun1_t<Variable1, Derived, Variable2> > sampler_1_func;

    /// @brief A \c UnaryFunctor type which returns a sample of \f$ x_2 \f$ given a value of \f$ x_1 \f$.
    typedef std::binder1st<std::mem_fun1_t<Variable2, Derived, Variable1> > sampler_2_func;

    mem_fun_two_var_cond_sampler()
        : two_var_cond_sampler<sampler_1_func, sampler_2_func>(sampler_1(), sampler_2())
    { }

private:
    inline sampler_1_func sampler_1() const
    {
        return std::bind1st(std::mem_fun(&Derived::sample_variable_1), this);
    }

    inline sampler_2_func sampler_2() const
    {
        return std::bind1st(std::mem_fun(&Derived::sample_variable_2), this);
    }
};

} } } // biggles::sampling::gibbs

#endif // SAMPLING_GIBBS_HPP__