/* boost random/linear_congruential.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Distributed under the Boost Software License, Version 1.0. (See
 * accompanying file LICENSE_1_0.txt or copy at
 * http://www.boost.org/LICENSE_1_0.txt)
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: linear_congruential.hpp 60755 2010-03-22 00:45:06Z steven_watanabe $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP
#define BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP

#include <iostream>
#include <cassert>
#include <stdexcept>
#include <boost/config.hpp>
#include <boost/limits.hpp>
#include <boost/static_assert.hpp>
#include <boost/random/detail/config.hpp>
#include <boost/random/detail/const_mod.hpp>
#include <boost/detail/workaround.hpp>

#include <boost/random/detail/disable_warnings.hpp>

namespace boost {
namespace random {

/**
 * Instantiations of class template linear_congruential model a
 * \pseudo_random_number_generator. Linear congruential pseudo-random
 * number generators are described in:
 *
 *  "Mathematical methods in large-scale computing units", D. H. Lehmer,
 *  Proc. 2nd Symposium on Large-Scale Digital Calculating Machines,
 *  Harvard University Press, 1951, pp. 141-146
 *
 * Let x(n) denote the sequence of numbers returned by some pseudo-random
 * number generator. Then for the linear congruential generator,
 * x(n+1) := (a * x(n) + c) mod m. Parameters for the generator are
 * x(0), a, c, m. The template parameter IntType shall denote an integral
 * type. It must be large enough to hold values a, c, and m. The template
 * parameters a and c must be smaller than m.
 *
 * Note: The quality of the generator crucially depends on the choice of
 * the parameters. User code should use one of the sensibly parameterized
 * generators such as minstd_rand instead.
 */
template<class IntType, IntType a, IntType c, IntType m, IntType val>
class linear_congruential
{
public:
  typedef IntType result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const result_type min_value = ( c == 0 ? 1 : 0 );
  static const result_type max_value = m-1;
#else
  BOOST_STATIC_CONSTANT(bool, has_fixed_range = false);
#endif
  BOOST_STATIC_CONSTANT(IntType, multiplier = a);
  BOOST_STATIC_CONSTANT(IntType, increment = c);
  BOOST_STATIC_CONSTANT(IntType, modulus = m);

  // MSVC 6 and possibly others crash when encountering complicated integral
  // constant expressions.  Avoid the check for now.
  // BOOST_STATIC_ASSERT(m == 0 || a < m);
  // BOOST_STATIC_ASSERT(m == 0 || c < m);

  /**
   * Constructs a linear_congruential generator, seeding it with @c x0.
   */
  explicit linear_congruential(IntType x0 = 1)
  {
    seed(x0);

    // MSVC fails BOOST_STATIC_ASSERT with std::numeric_limits at class scope
#ifndef BOOST_NO_LIMITS_COMPILE_TIME_CONSTANTS
    BOOST_STATIC_ASSERT(std::numeric_limits<IntType>::is_integer);
#endif
  }

  /**
   * Constructs a @c linear_congruential generator and seeds it
   * with values taken from the itrator range [first, last)
   * and adjusts first to point to the element after the last one
   * used.  If there are not enough elements, throws @c std::invalid_argument.
   *
   * first and last must be input iterators.
   */
  template<class It>
  linear_congruential(It& first, It last)
  {
      seed(first, last);
  }

  // compiler-generated copy constructor and assignment operator are fine

  /**
   * If c mod m is zero and x0 mod m is zero, changes the current value of
   * the generator to 1. Otherwise, changes it to x0 mod m. If c is zero,
   * distinct seeds in the range [1,m) will leave the generator in distinct
   * states. If c is not zero, the range is [0,m).
   */
  void seed(IntType x0 = 1)
  {
    // wrap _x if it doesn't fit in the destination
    if(modulus == 0) {
      _x = x0;
    } else {
      _x = x0 % modulus;
    }
    // handle negative seeds
    if(_x <= 0 && _x != 0) {
      _x += modulus;
    }
    // adjust to the correct range
    if(increment == 0 && _x == 0) {
      _x = 1;
    }
    assert(_x >= (min)());
    assert(_x <= (max)());
  }

  /**
   * seeds a @c linear_congruential generator with values taken
   * from the itrator range [first, last) and adjusts @c first to
   * point to the element after the last one used.  If there are
   * not enough elements, throws @c std::invalid_argument.
   *
   * @c first and @c last must be input iterators.
   */
  template<class It>
  void seed(It& first, It last)
  {
    if(first == last)
      throw std::invalid_argument("linear_congruential::seed");
    seed(*first++);
  }

  /**
   * Returns the smallest value that the @c linear_congruential generator
   * can produce.
   */
  result_type min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return c == 0 ? 1 : 0; }
  /**
   * Returns the largest value that the @c linear_congruential generator
   * can produce.
   */
  result_type max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return modulus-1; }

  /** Returns the next value of the @c linear_congruential generator. */
  IntType operator()()
  {
    _x = const_mod<IntType, m>::mult_add(a, _x, c);
    return _x;
  }

  static bool validation(IntType x) { return val == x; }

#ifdef BOOST_NO_OPERATORS_IN_NAMESPACE

  // Use a member function; Streamable concept not supported.
  bool operator==(const linear_congruential& rhs) const
  { return _x == rhs._x; }
  bool operator!=(const linear_congruential& rhs) const
  { return !(*this == rhs); }

#else
  friend bool operator==(const linear_congruential& x,
                         const linear_congruential& y)
  { return x._x == y._x; }
  friend bool operator!=(const linear_congruential& x,
                         const linear_congruential& y)
  { return !(x == y); }

#if !defined(BOOST_RANDOM_NO_STREAM_OPERATORS) && !BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os,
             const linear_congruential& lcg)
  {
    return os << lcg._x;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is,
             linear_congruential& lcg)
  {
    return is >> lcg._x;
  }

private:
#endif
#endif

  IntType _x;
};

// probably needs the "no native streams" caveat for STLPort
#if !defined(__SGI_STL_PORT) && BOOST_WORKAROUND(__GNUC__, == 2)
template<class IntType, IntType a, IntType c, IntType m, IntType val>
std::ostream&
operator<<(std::ostream& os,
           const linear_congruential<IntType,a,c,m,val>& lcg)
{
    return os << lcg._x;
}

template<class IntType, IntType a, IntType c, IntType m, IntType val>
std::istream&
operator>>(std::istream& is,
           linear_congruential<IntType,a,c,m,val>& lcg)
{
    return is >> lcg._x;
}
#elif defined(BOOST_RANDOM_NO_STREAM_OPERATORS) || BOOST_WORKAROUND(__BORLANDC__, BOOST_TESTED_AT(0x551))
template<class CharT, class Traits, class IntType, IntType a, IntType c, IntType m, IntType val>
std::basic_ostream<CharT,Traits>&
operator<<(std::basic_ostream<CharT,Traits>& os,
           const linear_congruential<IntType,a,c,m,val>& lcg)
{
    return os << lcg._x;
}

template<class CharT, class Traits, class IntType, IntType a, IntType c, IntType m, IntType val>
std::basic_istream<CharT,Traits>&
operator>>(std::basic_istream<CharT,Traits>& is,
           linear_congruential<IntType,a,c,m,val>& lcg)
{
    return is >> lcg._x;
}
#endif

#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
//  A definition is required even for integral static constants
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const bool linear_congruential<IntType, a, c, m, val>::has_fixed_range;
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const typename linear_congruential<IntType, a, c, m, val>::result_type linear_congruential<IntType, a, c, m, val>::min_value;
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const typename linear_congruential<IntType, a, c, m, val>::result_type linear_congruential<IntType, a, c, m, val>::max_value;
template<class IntType, IntType a, IntType c, IntType m, IntType val>
const IntType linear_congruential<IntType,a,c,m,val>::modulus;
#endif

} // namespace random

// validation values from the publications
/**
 * The specialization \minstd_rand0 was originally suggested in
 *
 *  @blockquote
 *  A pseudo-random number generator for the System/360, P.A. Lewis,
 *  A.S. Goodman, J.M. Miller, IBM Systems Journal, Vol. 8, No. 2,
 *  1969, pp. 136-146
 *  @endblockquote
 *
 * It is examined more closely together with \minstd_rand in
 *
 *  @blockquote
 *  "Random Number Generators: Good ones are hard to find",
 *  Stephen K. Park and Keith W. Miller, Communications of
 *  the ACM, Vol. 31, No. 10, October 1988, pp. 1192-1201
 *  @endblockquote
 */
typedef random::linear_congruential<int32_t, 16807, 0, 2147483647,
  1043618065> minstd_rand0;

/** The specialization \minstd_rand was suggested in
 *
 *  @blockquote
 *  "Random Number Generators: Good ones are hard to find",
 *  Stephen K. Park and Keith W. Miller, Communications of
 *  the ACM, Vol. 31, No. 10, October 1988, pp. 1192-1201
 *  @endblockquote
 */
typedef random::linear_congruential<int32_t, 48271, 0, 2147483647,
  399268537> minstd_rand;

#if !defined(BOOST_NO_INT64_T) && !defined(BOOST_NO_INTEGRAL_INT64_T)
/** Class @c rand48 models a \pseudo_random_number_generator. It uses
 * the linear congruential algorithm with the parameters a = 0x5DEECE66D,
 * c = 0xB, m = 2**48. It delivers identical results to the @c lrand48()
 * function available on some systems (assuming lcong48 has not been called).
 *
 * It is only available on systems where @c uint64_t is provided as an
 * integral type, so that for example static in-class constants and/or
 * enum definitions with large @c uint64_t numbers work.
 */
class rand48
{
public:
  typedef int32_t result_type;
#ifndef BOOST_NO_INCLASS_MEMBER_INITIALIZATION
  static const bool has_fixed_range = true;
  static const int32_t min_value = 0;
  static const int32_t max_value = integer_traits<int32_t>::const_max;
#else
  enum { has_fixed_range = false };
#endif
  /**
   * Returns the smallest value that the generator can produce
   */
  int32_t min BOOST_PREVENT_MACRO_SUBSTITUTION () const { return 0; }
  /**
   * Returns the largest value that the generator can produce
   */
  int32_t max BOOST_PREVENT_MACRO_SUBSTITUTION () const { return std::numeric_limits<int32_t>::max BOOST_PREVENT_MACRO_SUBSTITUTION (); }

#ifdef BOOST_RANDOM_DOXYGEN
  /**
   * If T is an integral type smaller than int46_t, constructs
   * a \rand48 generator with x(0) := (x0 << 16) | 0x330e.  Otherwise
   * constructs a \rand48 generator with x(0) = x0.
   */
  template<class T> explicit rand48(T x0 = 1);
#else
  rand48() : lcf(cnv(static_cast<int32_t>(1))) {}
  template<class T> explicit rand48(T x0) : lcf(cnv(x0)) { }
#endif
  template<class It> rand48(It& first, It last) : lcf(first, last) { }

  // compiler-generated copy ctor and assignment operator are fine

#ifdef BOOST_RANDOM_DOXYGEN
  /**
   * If T is an integral type smaller than int46_t, changes
   * the current value x(n) of the generator to (x0 << 16) | 0x330e.
   * Otherwise changes the current value x(n) to x0.
   */
  template<class T> void seed(T x0 = 1);
#else
  void seed() { seed(static_cast<int32_t>(1)); }
  template<class T> void seed(T x0) { lcf.seed(cnv(x0)); }
#endif
  template<class It> void seed(It& first, It last) { lcf.seed(first,last); }

  /**
   * Returns the next value of the generator.
   */
  int32_t operator()() { return static_cast<int32_t>(lcf() >> 17); }
  // by experiment from lrand48()
  static bool validation(int32_t x) { return x == 1993516219; }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE

#ifndef BOOST_RANDOM_NO_STREAM_OPERATORS
  template<class CharT,class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const rand48& r)
  { os << r.lcf; return os; }

  template<class CharT,class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, rand48& r)
  { is >> r.lcf; return is; }
#endif

  friend bool operator==(const rand48& x, const rand48& y)
  { return x.lcf == y.lcf; }
  friend bool operator!=(const rand48& x, const rand48& y)
  { return !(x == y); }
#else
  // Use a member function; Streamable concept not supported.
  bool operator==(const rand48& rhs) const
  { return lcf == rhs.lcf; }
  bool operator!=(const rand48& rhs) const
  { return !(*this == rhs); }
#endif
private:
  /// \cond hide_private_members
  random::linear_congruential<uint64_t,
    uint64_t(0xDEECE66DUL) | (uint64_t(0x5) << 32), // xxxxULL is not portable
    0xB, uint64_t(1)<<48, /* unknown */ 0> lcf;
  template<class T>
  static uint64_t cnv(T x)
  {
    if(sizeof(T) < sizeof(uint64_t)) {
      return (static_cast<uint64_t>(x) << 16) | 0x330e;
    } else {
      return(static_cast<uint64_t>(x));
    }
  }
  static uint64_t cnv(float x) { return(static_cast<uint64_t>(x)); }
  static uint64_t cnv(double x) { return(static_cast<uint64_t>(x)); }
  static uint64_t cnv(long double x) { return(static_cast<uint64_t>(x)); }
  /// \endcond
};
#endif /* !BOOST_NO_INT64_T && !BOOST_NO_INTEGRAL_INT64_T */

} // namespace boost

#include <boost/random/detail/enable_warnings.hpp>

#endif // BOOST_RANDOM_LINEAR_CONGRUENTIAL_HPP


#include "tkdCmdParser.h"

#include <stdlib.h>
#include <math.h>
#include <fstream>

#include "graphCommon.h"

#include "itkImageIteratorWithIndex.h"
#include "itkImageLinearIteratorWithIndex.h"
#include "itkImageRegionIteratorWithIndex.h"
#include "itkImageFileReader.h"

#include <boost/random/uniform_int.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/variate_generator.hpp>
#include <boost/random.hpp>

/**
 * wim@invivonmr.uu.nl, 31-07-2010.
 */
namespace glmm
{
	typedef double PixelType;
	typedef std::vector< PixelType > VectorType;
	typedef std::vector< VectorType > DataContainerType;

	typedef itk::Image< PixelType, 3 > ImageType;
	typedef itk::ImageFileReader< ImageType > ImageFileReaderType;
	typedef itk::ImageRegionConstIteratorWithIndex< ImageType > IteratorType;

	typedef boost::mt19937 random_number_type;
	typedef boost::uniform_int< int > int_distribution_type;
	typedef boost::variate_generator< random_number_type&, int_distribution_type> int_generator_type;

	// *******************************************************************************

	/**
	 * Option list.
	 */
	struct glmmParameters
	{
		std::vector< std::string > inputFileNames;
		std::vector< std::string > labelFileNames;
		std::string outputFileName;

		std::vector< std::string > inputNames;
		std::vector< std::string > labelNames;
		std::vector< std::string > covariateNames;
		std::vector< PixelType > covariateValues;

		int seed;

		bool allVoxels;
	};

	// *******************************************************************************

	/**
	 *
	 */
	class GLMMSelectVoxels
	{

	public:

		/**
		 * Run GLMM select voxel application.
		 */
		void Run( glmmParameters& args )
		{
			// Check inputs.
			Checks( args );

			// Read inputs.
			ReadImages( args.inputFileNames );

			// Read labels.
			ReadLabels( args.labelFileNames );

			// Create datacontainer.
			ReadData();

			// Get minimal vector size and index.
			std::vector< unsigned int > sizes;
			for( unsigned int label = 0; label < m_Data.size(); label++ )
				sizes.push_back( ( m_Data[ label ][ 0 ] ).size() );

			std::vector< unsigned int >::iterator min_it = std::min_element( sizes.begin(), sizes.end() );

			unsigned int minIndex = std::distance( sizes.begin(), min_it );
			unsigned int minSize = sizes[ minIndex ];

			// Resize all matrices so all have equal size.
			if( ! args.allVoxels )
			{
				ResampleDataContainer( minSize, minIndex, args.seed );
			}

			// Add optional covariates
			AddCovariates( args.covariateValues );

			// Write datacontainer to csv.
			WriteCSV( args );

			exit( EXIT_SUCCESS );
		}

	private:

		std::vector< ImageType::Pointer > m_Images;
		std::vector< ImageType::Pointer > m_Labels;
		std::vector< DataContainerType > m_Data;

		/**
		 * Add covariates to data containers.
		 */
		void AddCovariates( const std::vector< PixelType >& values )
		{
			for( unsigned int m = 0; m < m_Data.size(); m++ )
			{
				for( unsigned c = 0; c < values.size(); c++ )
				{
					VectorType covariate( ( m_Data.at( m ) ).at( 0 ).size(), values.at( c ) );
					m_Data.at( m ).push_back( covariate );
				}
			}
		}

		/**
		 * Write matrices to csv.
		 */
		void WriteCSV( const glmmParameters& args )
		{
			// open stream
			std::ofstream out( args.outputFileName.c_str() );
			if ( out.fail() )
			{
				std::cerr << "*** ERROR ***: Could not write to '" << args.outputFileName << "'!" << std::endl;
				exit( EXIT_FAILURE );
			}

			// write header
			std::vector< std::string > names;
			names.push_back( "Label" );

			std::back_insert_iterator< std::vector< std::string > > p( names );

			copy( args.inputNames.begin( ), args.inputNames.end( ), p );
			copy( args.covariateNames.begin( ), args.covariateNames.end( ), p );

			for( unsigned int i = 0; i < names.size(); i++ )
			{
				out << names.at( i );
				if( i < names.size() - 1 )
					out << ",";
			}
			out << std::endl; // end header

			// write data

			for( unsigned l = 0; l < m_Data.size(); l++ )
			{
				DataContainerType data = m_Data.at( l );

				unsigned int cols = data.size();
				unsigned int rows = data.at( 0 ).size();

				for( unsigned r = 0; r < rows; r++ )
				{
					out << args.labelNames.at( l ) << ",";
					for( unsigned c = 0; c < cols; c++ )
					{
						out << data[c][r];

						if( c < cols - 1 )
							out << ",";
					}
					out << std::endl;
				}
			}

			out.close();
		}

		/**
		 * Resample matrices, so they match the smallest one in size.
		 */
		void ResampleDataContainer( unsigned int minSize, unsigned int minIndex, int seed )
		{
			for( unsigned int label = 0; label < m_Data.size(); label++ )
			{
				if( label == minIndex )
					continue; // skip subsampling smallest label
				else
				{
					DataContainerType data = m_Data.at( label );

					for( unsigned int img = 0; img < data.size(); img++ )
						data.at( img ) = GetRandomValues( data.at( img ), minSize, seed );

					m_Data.at( label ) = data;
				}
			}
		}

		/**
		 * Read data containers (for each image one row) for each label image.
		 */
		void ReadData()
		{
			for( unsigned int label = 0; label < m_Labels.size(); label++ )
			{
				DataContainerType data;

				for( unsigned int image = 0; image < m_Images.size(); image++ )
				{
					VectorType vector;

					IteratorType it( m_Images.at( image ), m_Images.at( image )->GetLargestPossibleRegion() );

					for ( it.GoToBegin(); !it.IsAtEnd(); ++it )
					{
						PixelType takePixel = m_Labels.at( label )->GetPixel( it.GetIndex() );

						if ( takePixel != 0 )
							vector.push_back( it.Get() );
					}
					data.push_back( vector );
				}
				m_Data.push_back( data );
			}
		}

		/**
		 * Allocate and read global images.
		 */
		void ReadImages( const std::vector< std::string >& images )
		{
			for( unsigned int i = 0; i < images.size(); i++ )
				m_Images.push_back( ReadImage( images.at( i ) ) );
		}

		/**
		 * Allocate and read global labels.
		 */
		void ReadLabels( const std::vector< std::string >& images )
		{
			for( unsigned int i = 0; i < images.size(); i++ )
				m_Labels.push_back( ReadImage( images.at( i ) ) );
		}

		/**
		 * Read image.
		 */
		ImageType::Pointer ReadImage( const std::string& image )
		{
			ImageFileReaderType::Pointer reader = ImageFileReaderType::New();
			reader->SetFileName( image );
			reader->Update();
			return reader->GetOutput();
		}

		/**
		 * Return uniform random values vector from inputs, with similar size.
		 */
		VectorType GetRandomValues( const VectorType& input, unsigned int total, int seed )
		{
			// create generator ...
			random_number_type generator( seed );

			VectorType result;

			// uniform distribution: 0 -> largest index ...
			int_distribution_type int_uni_dist( 0, input.size() - 1 );

			// generator ...
			int_generator_type int_distribution( generator, int_uni_dist );

			for ( unsigned int i = 0; i < total; i++ )
			{
				//result.push_back( int_distribution() );
				result.push_back( input.at( int_distribution() ) );
			}
			return result;
		}

		/**
		 * Checks.
		 */
		void Checks( glmmParameters& args )
		{
			// if names not specified, use fileNames instead.
			if( args.inputNames.empty() )
				args.inputNames = args.inputFileNames;

			if( args.labelNames.empty() )
				args.labelNames = args.labelFileNames;

			// input names size
			if( args.inputNames.size() != args.inputFileNames.size() )
			{
				std::cerr << "*** ERROR ***: input names and input files different is size!" << std::endl;
				exit( EXIT_FAILURE );
			}

			// label names size
			if( args.labelNames.size() != args.labelFileNames.size() )
			{
				std::cerr << "*** ERROR ***: label names and label files different is size!" << std::endl;
				exit( EXIT_FAILURE );
			}

			// covariate size
			if( args.covariateNames.size() != args.covariateValues.size() )
			{
				std::cerr << "*** ERROR ***: covariate names and values different is size!" << std::endl;
				exit( EXIT_FAILURE );
			}
			// image dims
			for( unsigned int i = 0; i < args.inputFileNames.size(); i++ )
			{
				if( graph::Graph< PixelType >::GetImageDimensions( args.inputFileNames.at( i ) ) != 3 )
				{
					std::cerr << "*** ERROR ***: not all input images are 3D!" << std::endl;
					exit( EXIT_FAILURE );
				}
			}
			// labels dims
			for( unsigned int i = 0; i < args.labelFileNames.size(); i++ )
			{
				if( graph::Graph< PixelType >::GetImageDimensions( args.labelFileNames.at( i ) ) != 3 )
				{
					std::cerr << "*** ERROR ***: not all label images are 3D!" << std::endl;
					exit( EXIT_FAILURE );
				}
			}
		}

	};

} // end namespace glmm

/**
 * Main.
 */
int main( int argc, char ** argv )
{
	tkd::CmdParser p( argv[0], "Create csv output for GLMM R." );

	glmm::glmmParameters args;

	args.seed = 100;
	args.allVoxels = false;

	p.AddArgument( args.inputFileNames, "inputs" )
			->AddAlias( "i" )
			->SetDescription( "3D Input files" )
			->SetMinMax( 1,10000 )
			->SetRequired( true );

	p.AddArgument( args.labelFileNames, "labels" )
			->AddAlias( "l" )
			->SetDescription( "3D Label files" )
			->SetMinMax( 1, 10000 )
			->SetRequired( true );

	p.AddArgument( args.outputFileName, "output" )
			->AddAlias( "o" )
			->SetDescription( "output CSV file" )
			->SetMinMax( 1,1 )
			->SetRequired( true );

	p.AddArgument( args.inputNames, "input-names" )
			->AddAlias( "in" )
			->SetDescription( "3D Input file names (default: inputFileNames)" )
			->SetMinMax( 1,10000 );

	p.AddArgument( args.labelNames, "label-names" )
			->AddAlias( "ln" )
			->SetDescription( "3D Label file names (default: labelFileNames)" )
			->SetMinMax( 1,10000 );

	p.AddArgument( args.covariateNames, "covariate-names" )
			->AddAlias( "cn" )
			->SetDescription( "Names optional covariates" )
			->SetMinMax( 1,10000 );

	p.AddArgument( args.covariateValues, "covariate-values" )
			->AddAlias( "cv" )
			->SetDescription( "Values optional covariates" )
			->SetMinMax( 1,10000 );

	p.AddArgument( args.seed, "seed" )
			->AddAlias( "s" )
			->SetDescription( "Seed (default: 100" )
			->SetMinMax( 1,1 );

	p.AddArgument( args.allVoxels, "all-voxels" )
			->AddAlias( "a" )
			->SetDescription( "Include all voxels (default: equal number of voxels per label" );

	if ( !p.Parse( argc, argv ) )
	{
		p.PrintUsage( std::cout );
		return EXIT_FAILURE;
	}

	glmm::GLMMSelectVoxels glmmSelectVoxels;

	glmmSelectVoxels.Run( args );

	return EXIT_SUCCESS;
}


//  MIT License
//
//  Copyright (c) 2020 Paul O. Lewis
//
//  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.
#pragma once

#include <ctime>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/uniform_real.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/gamma_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace termite {

    class Lot {
        public:
                                            Lot();
                                            ~Lot();

            void                            setSeed(unsigned seed);
            double                          uniform();
            int                             randint(int low, int high);
            double                          normal();
            double                          gamma(double shape, double scale);
            double                          logUniform();

            typedef std::shared_ptr<Lot>    SharedPtr;

        private:

            typedef boost::variate_generator<boost::mt19937 &, boost::random::uniform_01<> >                uniform_variate_generator_t;
            typedef boost::variate_generator<boost::mt19937 &, boost::random::normal_distribution<> >       normal_variate_generator_t;
            typedef boost::variate_generator<boost::mt19937 &, boost::random::gamma_distribution<> >        gamma_variate_generator_t;
            typedef boost::variate_generator<boost::mt19937 &, boost::random::uniform_int_distribution<> >  uniform_int_generator_t;

            unsigned                                        _seed;
            boost::mt19937                                  _generator;
            std::shared_ptr<uniform_variate_generator_t>    _uniform_variate_generator;
            std::shared_ptr<normal_variate_generator_t>     _normal_variate_generator;
            std::shared_ptr<gamma_variate_generator_t>      _gamma_variate_generator;
            std::shared_ptr<uniform_int_generator_t>        _uniform_int_generator;

            double                                          _gamma_shape;
            int                                             _low;
            int                                             _high;
    };

    // member function bodies go here

    inline Lot::Lot() : _seed(0), _gamma_shape(1.0), _low(0), _high(100) {
        //std::cout << "Constructing a Lot" << std::endl;
        _generator.seed(static_cast<unsigned int>(std::time(0)));
        _uniform_variate_generator = std::shared_ptr<uniform_variate_generator_t>(new uniform_variate_generator_t(_generator, boost::random::uniform_01<>()));
        _normal_variate_generator = std::shared_ptr<normal_variate_generator_t>(new normal_variate_generator_t(_generator, boost::random::normal_distribution<>()));
        _gamma_variate_generator = std::shared_ptr<gamma_variate_generator_t>(new gamma_variate_generator_t(_generator, boost::random::gamma_distribution<>(_gamma_shape)));
        _uniform_int_generator = std::shared_ptr<uniform_int_generator_t>(new uniform_int_generator_t(_generator, boost::random::uniform_int_distribution<>(_low, _high)));
    }

    inline Lot::~Lot() {
        //std::cout << "Destroying a Lot" << std::endl;
        _uniform_variate_generator.reset();
        _normal_variate_generator.reset();
        _gamma_variate_generator.reset();
        _uniform_int_generator.reset();
    }

    inline void Lot::setSeed(unsigned seed) {
        _seed = seed;
        _generator.seed(_seed > 0 ? _seed : static_cast<unsigned int>(std::time(0)));
    }

    inline double Lot::uniform() {
        double u = (*_uniform_variate_generator)();
        while (u <= 0.0)
            u = (*_uniform_variate_generator)();
        return u;
    }

    inline double Lot::logUniform() {
        double u = (*_uniform_variate_generator)();
        while (u <= 0.0)
            u = (*_uniform_variate_generator)();
        return std::log(u);
    }

    inline double Lot::normal() {
        return (*_normal_variate_generator)();
    }

    inline double Lot::gamma(double shape, double scale) {
        assert(shape > 0.0);
        assert(scale > 0.0);
        if (shape != _gamma_shape) {
            _gamma_shape = shape;
            _gamma_variate_generator.reset(new gamma_variate_generator_t(_generator, boost::random::gamma_distribution<>(_gamma_shape,1)));
        }
        double deviate = (*_gamma_variate_generator)();
        return scale*deviate;
    }

    inline int Lot::randint(int low, int high) {
        if (low != _low || high != _high) {
            _low  = low;
            _high = high;
            _uniform_int_generator.reset(new uniform_int_generator_t(_generator, boost::random::uniform_int_distribution<>(_low,_high)));
        }
        return (*_uniform_int_generator)();
    }

}


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

#include <boost/test/unit_test.hpp>
#include <boost/iterator/counting_iterator.hpp>
#include <boost/time_series/sparse_series.hpp>
#include <boost/time_series/dense_series.hpp>
#include <boost/time_series/piecewise_constant_series.hpp>
#include <boost/time_series/ordered_inserter.hpp>
#include <boost/time_series/numeric/subscript.hpp>
#include "./is_close.hpp"

using namespace boost;
using namespace unit_test;

///////////////////////////////////////////////////////////////////////////////
// test_subscript
//
void test_subscript()
{
    using namespace time_series;
    typedef counting_iterator<int> int_;

    dense_series<int> d;
    std::copy(int_(0), int_(100), make_ordered_inserter(d, 0)).commit();

    sparse_series<int> indices;
    make_ordered_inserter(indices)
        (1, 0)(10, 10)(20, 20)(30, 30)(40, 40)(50, 49)
    .commit();

    sparse_series<int> result;
    make_ordered_inserter(result)
        (1, 0)(20, 10)(40, 20)(60, 30)(80, 40)(99, 49)
    .commit();

    BOOST_CHECK_EQUAL(result, subscript(d, indices));
}

///////////////////////////////////////////////////////////////////////////////
// test_floating_point_subscript
//
void test_floating_point_subscript()
{
    using namespace time_series;

    piecewise_constant_series<double, int, double> pwc;
    make_ordered_inserter(pwc)
        (1., 00., 10.)(2., 10., 20.)(3., 20., 30.)(4., 30., 40.)(5., 40., 50.)
        (6., 50., 60.)(7., 60., 70.)(8., 70., 80.)(9., 80., 90.)(10., 90., 100.)
    .commit();

    sparse_series<double, int, double> indices;
    make_ordered_inserter(indices)
        (1.5, 0.)(10.5, 10.)(20.5, 20.)(30.5, 30.)(40.5, 40.)(50.5, 49.)
    .commit();

    sparse_series<double, int, double> expected;
    make_ordered_inserter(expected)
        (1., 0.)(3., 10.)(5., 20.)(7., 30.)(9., 40.)(10., 49)
    .commit();

    sparse_series<double, int, double> actual = subscript(pwc, indices);
    BOOST_CHECK(is_close(expected, actual, 0.1));
}

///////////////////////////////////////////////////////////////////////////////
// init_unit_test_suite
//
test_suite* init_unit_test_suite( int argc, char* argv[] )
{
    test_suite *test = BOOST_TEST_SUITE("subscript test");

    test->add(BOOST_TEST_CASE(&test_subscript));
    test->add(BOOST_TEST_CASE(&test_floating_point_subscript));

    return test;
}


#include "zinfandel.h"

#include "parse_args.h"
#include "tensorOps.h"
#include "threads.h"

#include <Eigen/SVD>
#include <complex>

// Helper Functions
Eigen::MatrixXcf GrabSources(
  Cx3 const &ks,
  float const scale,
  Index const n_src,
  Index const s_read,
  Index const n_read,
  std::vector<Index> const &spokes)
{
  assert((s_read + n_read + n_src) < ks.dimension(1));
  Index const n_chan = ks.dimension(0);
  Index const n_spoke = spokes.size();
  Eigen::MatrixXcf S(n_chan * n_src, n_read * n_spoke);
  S.setZero();
  for (Index i_spoke = 0; i_spoke < n_spoke; i_spoke++) {
    Index const col_spoke = i_spoke * n_read;
    Index const ind_spoke = spokes[i_spoke];
    assert(ind_spoke < ks.dimension(2));
    for (Index i_read = 0; i_read < n_read; i_read++) {
      Index const col = col_spoke + i_read;
      for (Index i_coil = 0; i_coil < n_chan; i_coil++) {
        Index const row_coil = i_coil * n_src;
        for (Index i_src = 0; i_src < n_src; i_src++) {
          Index const row = row_coil + i_src;
          S(row, col) = ks(i_coil, s_read + i_read + i_src, ind_spoke) / scale;
        }
      }
    }
  }
  return S;
}

Eigen::MatrixXcf GrabTargets(
  Cx3 const &ks,
  float const scale,
  Index const s_read,
  Index const n_read,
  std::vector<Index> const &spokes)
{
  Index const n_chan = ks.dimension(0);
  Index const n_spoke = spokes.size();
  Eigen::MatrixXcf T(n_chan, n_read * n_spoke);
  T.setZero();
  for (Index i_spoke = 0; i_spoke < n_spoke; i_spoke++) {
    Index const col_spoke = i_spoke * n_read;
    Index const ind_spoke = spokes[i_spoke];
    for (Index i_read = 0; i_read < n_read; i_read++) {
      Index const col = col_spoke + i_read;
      for (Index i_coil = 0; i_coil < n_chan; i_coil++) {
        T(i_coil, col) = ks(i_coil, s_read + i_read, ind_spoke) / scale;
      }
    }
  }
  return T;
}

Eigen::MatrixXcf
CalcWeights(Eigen::MatrixXcf const &src, Eigen::MatrixXcf const tgt, float const lambda)
{
  if (lambda > 0.f) {
    auto const reg = lambda * src.norm() * Eigen::MatrixXcf::Identity(src.rows(), src.rows());
    auto const rhs = src * src.adjoint() + reg;
    Eigen::MatrixXcf pinv = rhs.completeOrthogonalDecomposition().pseudoInverse();
    return tgt * src.adjoint() * pinv;
  } else {
    Eigen::MatrixXcf const pinv = src.completeOrthogonalDecomposition().pseudoInverse();
    return tgt * pinv;
  }
}

std::vector<Index>
FindClosest(R3 const &traj, Index const &tgt, Index const &n_spoke, std::vector<Index> &all_spokes)
{
  std::vector<Index> spokes(n_spoke);
  R1 const end_is = traj.chip(tgt, 2).chip(traj.dimension(1) - 1, 1);
  std::partial_sort(
    all_spokes.begin(),
    all_spokes.begin() + n_spoke,
    all_spokes.end(),
    [&traj, end_is](Index const a, Index const b) {
      auto const &end_a = traj.chip(a, 2).chip(traj.dimension(1) - 1, 1);
      auto const &end_b = traj.chip(b, 2).chip(traj.dimension(1) - 1, 1);
      return Norm(end_a - end_is) < Norm(end_b - end_is);
    });
  std::copy_n(all_spokes.begin(), n_spoke, spokes.begin());
  return spokes;
}

// Actual calculation
void zinfandel(
  Index const gap_sz,
  Index const n_src,
  Index const n_spoke,
  Index const n_read1,
  float const lambda,
  R3 const &traj,
  Cx3 &ks,
  Log &log)
{
  Index const n_read = n_read1 < 1 ? ks.dimension(1) - (gap_sz + n_src) : n_read1;

  log.info(
    FMT_STRING("ZINFANDEL Gap {} Sources {} Cal Spokes/Read {}/{} "),
    gap_sz,
    n_src,
    n_spoke,
    n_read);

  for (Index ig = gap_sz; ig > 0; ig--) {
    auto spoke_task = [&](Index const spoke_lo, Index const spoke_hi) {
      std::vector<Index> all_spokes(ks.dimension(2)); // Need a thread-local copy of the indices
      std::iota(all_spokes.begin(), all_spokes.end(), 0L);
      for (auto is = spoke_lo; is < spoke_hi; is++) {
        float const scale = R0(ks.chip(is, 2).abs().maximum())();
        auto const spokes = FindClosest(traj, is, n_spoke, all_spokes);
        auto const calS = GrabSources(ks, scale, n_src, ig + 1, n_read, spokes);
        auto const calT = GrabTargets(ks, scale, ig, n_read, spokes);
        auto const W = CalcWeights(calS, calT, lambda);
        auto const S = GrabSources(ks, scale, n_src, ig, 1, {is});
        auto const T = W * S;
        for (Index icoil = 0; icoil < ks.dimension(0); icoil++) {
          ks(icoil, ig - 1, is) = T(icoil) * scale;
        }
      }
    };
    Threads::RangeFor(spoke_task, 0, ks.dimension(2));
  }
}


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/// @project        Open Space Toolkit ▸ Mathematics
/// @file           OpenSpaceToolkit/Mathematics/Geometry/2D/Objects/Polygon.cpp
/// @author         Lucas Brémond <lucas@loftorbital.com>
/// @license        Apache License 2.0

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

#include <OpenSpaceToolkit/Mathematics/Geometry/2D/Transformation.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/2D/Objects/MultiPolygon.hpp>
#include <OpenSpaceToolkit/Mathematics/Geometry/2D/Objects/Polygon.hpp>

#include <OpenSpaceToolkit/Core/Types/String.hpp>
#include <OpenSpaceToolkit/Core/Types/Size.hpp>
#include <OpenSpaceToolkit/Core/Types/Index.hpp>
#include <OpenSpaceToolkit/Core/Error.hpp>
#include <OpenSpaceToolkit/Core/Utilities.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>
#include <boost/geometry/strategies/transform/matrix_transformers.hpp>
#include <boost/geometry/strategies/transform.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry.hpp>
#include <boost/numeric/ublas/matrix.hpp>

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

namespace ostk
{
namespace math
{
namespace geom
{
namespace d2
{
namespace objects
{

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

using boost::geometry::cs::cartesian ;
using boost::geometry::model::point ;
using boost::geometry::model::ring ;
using boost::geometry::model::polygon ;

using ostk::core::types::Index ;
using ostk::core::types::Size ;
using ostk::core::types::String ;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

class Polygon::Impl
{

    public:

                                Impl                                        (   const   Array<Point>&               anOuterRing,
                                                                                const   Array<Array<Point>>&        anInnerRingArray                            ) ;

                                ~Impl                                       ( ) = default ;

        bool                    operator ==                                 (   const   Polygon::Impl&              aPolygon                                    ) const ;

        bool                    isDefined                                   ( ) const ;

        bool                    intersects                                  (   const   Polygon&                    aPolygon                                    ) const ;

        bool                    contains                                    (   const   Point&                      aPoint                                      ) const ;

        bool                    contains                                    (   const   PointSet&                   aPointSet                                   ) const ;

        Size                    getInnerRingCount                           ( ) const ;

        Array<Polygon::Vertex>  getOuterRingVertices                        ( ) const ;

        Array<Polygon::Vertex>  getInnerRingVerticesAt                      (   const   Index&                      aRingIndex                                  ) const ;

        Size                    getEdgeCount                                ( ) const ;

        Size                    getOuterRingEdgeCount                       ( ) const ;

        Size                    getVertexCount                              ( ) const ;

        Polygon::Ring           getOuterRing                                ( ) const ;

        Polygon::Ring           getInnerRingAt                              (   const   Index&                      anInnerRingIndex                            ) const ;

        Polygon::Edge           getEdgeAt                                   (   const   Index                       anEdgeIndex                                 ) const ;

        Polygon::Vertex         getVertexAt                                 (   const   Index                       aVertexIndex                                ) const ;

        Array<Polygon::Edge>    getEdges                                    ( ) const ;

        Array<Polygon::Vertex>  getVertices                                 ( ) const ;

        Polygon                 getConvexHull                               ( ) const ;

        // Intersection            intersectionWith                            (   const   Polygon&                    aPolygon                                    ) const ;

        String                  toString                                    (   const   Object::Format&             aFormat,
                                                                                const   Integer&                    aPrecision                                  ) const ;

        void                    applyTransformation                         (   const   Transformation&             aTransformation                             ) ;

    private:

        typedef                 point<double, 2, cartesian>                     BoostPoint ;
        typedef                 ring<Impl::BoostPoint>                          BoostRing ;
        typedef                 polygon<Impl::BoostPoint>                       BoostPolygon ;

        Impl::BoostPolygon      polygon_ ;

        static Impl::BoostPolygon BoostPolygonFromPoints                    (   const   Array<Point>&               aPointArray                                 ) ;

        static Polygon          PolygonFromBoostPolygon                     (   const   Polygon::Impl::BoostPolygon& aPolygon                                   ) ;

} ;

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                Polygon::Impl::Impl                         (   const   Array<Point>&               anOuterRing,
                                                                                const   Array<Array<Point>>&        anInnerRingArray                            )
                                :   polygon_(Polygon::Impl::BoostPolygonFromPoints(anOuterRing))
{

    for (const auto& innerRing : anInnerRingArray) // [TBM] This is temporary, should be constructed inline instead
    {

        if (innerRing.getSize() < 3)
        {
            throw ostk::core::error::RuntimeError("At least 3 points are necessary to define an inner ring.") ;
        }

        Polygon::Impl::BoostRing ring ;

        for (const auto& innerRingPoint : innerRing)
        {
            boost::geometry::append(ring, Polygon::Impl::BoostPoint(innerRingPoint.x(), innerRingPoint.y())) ;
        }

        boost::geometry::correct(ring) ;

        polygon_.inners().push_back(ring) ;

    }

}

bool                            Polygon::Impl::operator ==                  (   const   Polygon::Impl&              aPolygon                                    ) const
{
    return boost::geometry::equals(polygon_, aPolygon.polygon_) ;
}

bool                            Polygon::Impl::isDefined                    ( ) const
{
    return polygon_.outer().size() >= 3 ;
}

bool                            Polygon::Impl::intersects                   (   const   Polygon&                    aPolygon                                    ) const
{

    try
    {
        return boost::geometry::intersects(polygon_, aPolygon.implUPtr_->polygon_) ;
    }
    catch (const std::exception& anException)
    {
        throw ostk::core::error::RuntimeError("Error when checking if polygon intersects polygon: [{}]", anException.what()) ;
    }

    return false ;

}

bool                            Polygon::Impl::contains                     (   const   Point&                      aPoint                                      ) const
{

    try
    {
        return boost::geometry::covered_by(Polygon::Impl::BoostPoint(aPoint.x(), aPoint.y()), polygon_) ;
    }
    catch (const std::exception& anException)
    {
        throw ostk::core::error::RuntimeError("Error when checking if polygon contains point: [{}]", anException.what()) ;
    }

    return false ;

}

bool                            Polygon::Impl::contains                     (   const   PointSet&                   aPointSet                                   ) const
{

    for (const auto& point : aPointSet)
    {

        if (!this->contains(point))
        {
            return false ;
        }

    }

    return true ;

}

Size                            Polygon::Impl::getInnerRingCount            ( ) const
{
    return polygon_.inners().size() ;
}

Array<Polygon::Vertex>          Polygon::Impl::getOuterRingVertices         ( ) const
{

    if (polygon_.outer().empty())
    {
        return Array<Point>::Empty() ;
    }

    Array<Point> outerRingVertices = Array<Point>::Empty() ;

    for (size_t vertexIdx = 0; vertexIdx < (polygon_.outer().size() - 1); ++vertexIdx)
    {
        outerRingVertices.add(Point(boost::geometry::get<0>(polygon_.outer().at(vertexIdx)), boost::geometry::get<1>(polygon_.outer().at(vertexIdx)))) ;
    }

    return outerRingVertices ;

}

Array<Polygon::Vertex>          Polygon::Impl::getInnerRingVerticesAt       (   const   Index&                      aRingIndex                                  ) const
{

    if (aRingIndex >= this->getInnerRingCount())
    {
        throw ostk::core::error::RuntimeError("Inner ring index [{}] out of bounds [{}].", aRingIndex, this->getInnerRingCount()) ;
    }

    if (polygon_.inners().at(aRingIndex).empty())
    {
        return Array<Point>::Empty() ;
    }

    Array<Point> innerRingVertices = Array<Point>::Empty() ;

    for (size_t vertexIdx = 0; vertexIdx < (polygon_.inners().at(aRingIndex).size() - 1); ++vertexIdx)
    {
        innerRingVertices.add(Point(boost::geometry::get<0>(polygon_.inners().at(aRingIndex).at(vertexIdx)), boost::geometry::get<1>(polygon_.inners().at(aRingIndex).at(vertexIdx)))) ;
    }

    return innerRingVertices ;

}

Size                            Polygon::Impl::getEdgeCount                 ( ) const
{

    Size edgeCount = boost::geometry::num_points(polygon_.outer()) - 1 ;

    for (const auto& innerRing : polygon_.inners())
    {
        edgeCount += boost::geometry::num_points(innerRing) - 1 ;
    }

    return edgeCount ;

}

Size                            Polygon::Impl::getOuterRingEdgeCount        ( ) const
{
    return boost::geometry::num_points(polygon_.outer()) - 1 ;
}

Size                            Polygon::Impl::getVertexCount               ( ) const
{
    return boost::geometry::num_points(polygon_) - (1 + this->getInnerRingCount()) ;
}

Polygon::Ring                   Polygon::Impl::getOuterRing                 ( ) const
{

    Array<Point> ringVertices = this->getOuterRingVertices() ;

    if (!ringVertices.isEmpty())
    {
        ringVertices.add(ringVertices[0]) ;
    }

    return { ringVertices } ;

}

Polygon::Ring                   Polygon::Impl::getInnerRingAt               (   const   Index&                      anInnerRingIndex                            ) const
{

    Array<Point> ringVertices = this->getInnerRingVerticesAt(anInnerRingIndex) ;

    if (!ringVertices.isEmpty())
    {
        ringVertices.add(ringVertices[0]) ;
    }

    return { ringVertices } ;

}

Polygon::Edge                   Polygon::Impl::getEdgeAt                    (   const   Index                       anEdgeIndex                                 ) const
{

    if (anEdgeIndex >= this->getEdgeCount())
    {
        throw ostk::core::error::runtime::Wrong("Edge index") ;
    }

    if (anEdgeIndex >= this->getOuterRingEdgeCount())
    {
        throw ostk::core::error::runtime::ToBeImplemented("Inner ring edge access.") ;
    }

    const Point firstVertex = this->getVertexAt(anEdgeIndex) ;
    const Point secondVertex = (anEdgeIndex != (this->getOuterRingEdgeCount() - 1)) ? this->getVertexAt(anEdgeIndex + 1) : this->getVertexAt(0) ;

    return { firstVertex, secondVertex } ;

}

Polygon::Vertex                 Polygon::Impl::getVertexAt                  (   const   Index                       aVertexIndex                                ) const
{

    if (aVertexIndex >= (boost::geometry::num_points(polygon_) - 1))
    {
        throw ostk::core::error::runtime::Wrong("Vertex index") ;
    }

    if (aVertexIndex >= (polygon_.outer().size() - 1))
    {
        throw ostk::core::error::runtime::ToBeImplemented("Inner ring vertex access.") ;
    }

    return { boost::geometry::get<0>(polygon_.outer().at(aVertexIndex)), boost::geometry::get<1>(polygon_.outer().at(aVertexIndex)) } ;

}

Array<Polygon::Edge>            Polygon::Impl::getEdges                     ( ) const
{

    Array<Polygon::Edge> edges = Array<Polygon::Edge>::Empty() ;

    edges.reserve(this->getEdgeCount()) ;

    for (Index edgeIndex = 0; edgeIndex < this->getEdgeCount(); ++edgeIndex)
    {
        edges.add(this->getEdgeAt(edgeIndex)) ;
    }

    return edges ;

}

Array<Polygon::Vertex>          Polygon::Impl::getVertices                  ( ) const
{

    Array<Polygon::Vertex> vertices = Array<Polygon::Vertex>::Empty() ;

    for (const auto& vertex : this->getOuterRingVertices())
    {
        vertices.add(vertex) ;
    }

    for (Index innerRingIndex = 0; innerRingIndex < this->getInnerRingCount(); ++innerRingIndex)
    {
        for (const auto& vertex : this->getInnerRingVerticesAt(innerRingIndex))
        {
            vertices.add(vertex) ;
        }
    }

    return vertices ;

}

Polygon                         Polygon::Impl::getConvexHull                ( ) const
{

    Polygon::Impl::BoostPolygon convexHull ;

    try
    {
        boost::geometry::convex_hull(polygon_, convexHull) ;
    }
    catch (const std::exception& anException)
    {
        throw ostk::core::error::RuntimeError("Error caught while computing the convex hull: [{}]", anException.what()) ;
    }

    return Polygon::Impl::PolygonFromBoostPolygon(convexHull) ;

}

// Intersection                    Polygon::Impl::intersectionWith             (   const   Polygon&                    aPolygon                                    ) const
// {

// }

String                          Polygon::Impl::toString                     (   const   Object::Format&             aFormat,
                                                                                const   Integer&                    aPrecision                                  ) const
{

    switch (aFormat)
    {

        case Object::Format::Standard:
        case Object::Format::WKT:
        {

            std::stringstream stringStream ;

            if (aPrecision.isDefined())
            {
                stringStream << std::fixed << std::setprecision(aPrecision) << boost::geometry::wkt(polygon_) ;
            }
            else
            {
                stringStream << boost::geometry::wkt(polygon_) ;
            }

            return stringStream.str() ;

        }

        default:
            throw ostk::core::error::runtime::Wrong("Format") ;
            break ;

    }

    return String::Empty() ;

}

void                            Polygon::Impl::applyTransformation          (   const   Transformation&             aTransformation                             )
{

    using ostk::math::obj::Matrix3d ;

    Polygon::Impl::BoostPolygon transformedPolygon ;

    const Matrix3d transformationMatrix = aTransformation.getMatrix() ;

    const boost::geometry::strategy::transform::matrix_transformer<double, 2, 2> transform = { transformationMatrix(0, 0), transformationMatrix(0, 1), transformationMatrix(0, 2),
                                                                                transformationMatrix(1, 0), transformationMatrix(1, 1), transformationMatrix(1, 2),
                                                                                transformationMatrix(2, 0), transformationMatrix(2, 1), transformationMatrix(2, 2) } ;

    boost::geometry::transform(polygon_, transformedPolygon, transform) ;

    polygon_ = transformedPolygon ;

}

Polygon::Impl::BoostPolygon     Polygon::Impl::BoostPolygonFromPoints       (   const   Array<Point>&               aPointArray                                 )
{

    if ((!aPointArray.isEmpty()) && (aPointArray.getSize() < 3))
    {
        throw ostk::core::error::RuntimeError("At least 3 points are necessary to define a polygon.") ;
    }

    Polygon::Impl::BoostPolygon polygon ;

    for (const auto& point : aPointArray)
    {
        boost::geometry::append(polygon, Polygon::Impl::BoostPoint(point.x(), point.y())) ;
    }

    boost::geometry::correct(polygon) ;

    return polygon ;

}

Polygon                         Polygon::Impl::PolygonFromBoostPolygon      (   const   Polygon::Impl::BoostPolygon& aPolygon                                   )
{

    Array<Point> outerRing = Array<Point>::Empty() ;

    for (size_t vertexIdx = 0; vertexIdx < (aPolygon.outer().size() - 1); ++vertexIdx)
    {
        outerRing.add(Point(boost::geometry::get<0>(aPolygon.outer().at(vertexIdx)), boost::geometry::get<1>(aPolygon.outer().at(vertexIdx)))) ;
    }

    Array<Array<Point>> innerRings = Array<Array<Point>>::Empty() ;

    for (const auto& innerRing : aPolygon.inners())
    {

        Array<Point> innerRingVertices = Array<Point>::Empty() ;

        for (size_t vertexIdx = 0; vertexIdx < (innerRing.size() - 1); ++vertexIdx)
        {
            innerRingVertices.add(Point(boost::geometry::get<0>(innerRing.at(vertexIdx)), boost::geometry::get<1>(innerRing.at(vertexIdx)))) ;
        }

        innerRings.add(innerRingVertices) ;

    }

    return Polygon { outerRing, innerRings } ;

}

////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

                                Polygon::Polygon                            (   const   Array<Point>&               anOuterRing,
                                                                                const   Array<Array<Point>>&        anInnerRingArray                            )
                                :   Object(),
                                    implUPtr_(std::make_unique<Polygon::Impl>(anOuterRing, anInnerRingArray))
{

}

                                Polygon::Polygon                            (   const   Polygon&                    aPolygon                                    )
                                :   Object(aPolygon),
                                    implUPtr_((aPolygon.implUPtr_ != nullptr) ? new Polygon::Impl(*aPolygon.implUPtr_) : nullptr)
{

}

                                Polygon::~Polygon                           ( )
{

}

Polygon&                        Polygon::operator =                         (   const   Polygon&                    aPolygon                                    )
{

    if (this != &aPolygon)
    {

        Object::operator = (aPolygon) ;

        implUPtr_.reset((aPolygon.implUPtr_ != nullptr) ? new Polygon::Impl(*aPolygon.implUPtr_) : nullptr) ;

    }

    return *this ;

}

Polygon*                        Polygon::clone                              ( ) const
{
    return new Polygon(*this) ;
}

bool                            Polygon::operator ==                        (   const   Polygon&                    aPolygon                                    ) const
{

    if ((!this->isDefined()) || (!aPolygon.isDefined()))
    {
        return false ;
    }

    return (*implUPtr_) == (*aPolygon.implUPtr_) ;

}

bool                            Polygon::operator !=                        (   const   Polygon&                    aPolygon                                    ) const
{
    return !((*this) == aPolygon) ;
}

bool                            Polygon::isDefined                          ( ) const
{
    return (implUPtr_ != nullptr) && implUPtr_->isDefined() ;
}

bool                            Polygon::isNear                             (   const   Polygon&                    aPolygon,
                                                                                const   Real&                       aTolerance                                  ) const
{

    if (!aPolygon.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    if (!aTolerance.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Tolerance") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    if (this->getVertexCount() != aPolygon.getVertexCount())
    {
        return false ;
    }

    const Array<Polygon::Vertex> firstVertices = this->getVertices() ;
    const Array<Polygon::Vertex> secondVertices = aPolygon.getVertices() ;

    // for (const auto vertexTuple : ostk::core::ctnr::iterators::Zip(this->getVertices(), aPolygon.getVertices()))
    for (const auto vertexTuple : ostk::core::ctnr::iterators::Zip(firstVertices, secondVertices))
    {

        if (!std::get<0>(vertexTuple).isNear(std::get<1>(vertexTuple), aTolerance))
        {
            return false ;
        }

    }

    return true ;

}

bool                            Polygon::intersects                         (   const   Polygon&                    aPolygon                                    ) const
{

    if (!aPolygon.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Point") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->intersects(aPolygon) ;

}

bool                            Polygon::contains                           (   const   Point&                      aPoint                                      ) const
{

    if (!aPoint.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Point") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->contains(aPoint) ;

}

bool                            Polygon::contains                           (   const   PointSet&                   aPointSet                                   ) const
{

    if (!aPointSet.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Point set") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->contains(aPointSet) ;

}

Size                            Polygon::getInnerRingCount                  ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getInnerRingCount() ;

}

Size                            Polygon::getEdgeCount                       ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getEdgeCount() ;

}

Size                            Polygon::getVertexCount                     ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getVertexCount() ;

}

Polygon::Ring                   Polygon::getOuterRing                       ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getOuterRing() ;

}

Polygon::Ring                   Polygon::getInnerRingAt                     (   const   Index&                      anInnerRingIndex                            ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getInnerRingAt(anInnerRingIndex) ;

}

Polygon::Edge                   Polygon::getEdgeAt                          (   const   Index                       anEdgeIndex                                 ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getEdgeAt(anEdgeIndex) ;

}

Polygon::Vertex                 Polygon::getVertexAt                        (   const   Index                       aVertexIndex                                ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getVertexAt(aVertexIndex) ;

}

Array<Polygon::Edge>            Polygon::getEdges                           ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getEdges() ;

}

Array<Polygon::Vertex>          Polygon::getVertices                        ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getVertices() ;

}

Polygon                         Polygon::getConvexHull                      ( ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->getConvexHull() ;

}

void                            Polygon::print                              (           std::ostream&               anOutputStream,
                                                                                bool                        displayDecorators                           ) const
{

    displayDecorators ? ostk::core::utils::Print::Header(anOutputStream, "Polygon") : void () ;

    ostk::core::utils::Print::Separator(anOutputStream, "Outer Ring") ;

    if (implUPtr_ != nullptr)
    {

        for (const auto& point : implUPtr_->getOuterRingVertices())
        {
            ostk::core::utils::Print::Line(anOutputStream) << String::Format("- {}", point.toString()) ;
        }

    }
    else
    {
        ostk::core::utils::Print::Line(anOutputStream) << "Undefined" ;
    }

    ostk::core::utils::Print::Separator(anOutputStream, "Inner Rings") ;

    if (implUPtr_ != nullptr)
    {

        for (Index innerRingIndex = 0; innerRingIndex < implUPtr_->getInnerRingCount(); ++innerRingIndex)
        {

            ostk::core::utils::Print::Separator(anOutputStream, String::Format("Inner Ring @ {}", innerRingIndex)) ;

            for (const auto& point : implUPtr_->getInnerRingVerticesAt(innerRingIndex))
            {
                ostk::core::utils::Print::Line(anOutputStream) << String::Format("- {}", point.toString()) ;
            }

        }

    }
    else
    {
        ostk::core::utils::Print::Line(anOutputStream) << "Undefined" ;
    }

    displayDecorators ? ostk::core::utils::Print::Footer(anOutputStream) : void () ;

}

// Intersection                    Polygon::intersectionWith                   (   const   Polygon&                    aPolygon                                    ) const
// {

//     if ((!this->isDefined()) || (!aPolygon.isDefined()))
//     {
//         throw ostk::core::error::runtime::Undefined("Polygon") ;
//     }

//     return implUPtr_->intersectionWith(aPolygon) ;

// }

MultiPolygon                    Polygon::unionWith                          (   const   Polygon&                    aPolygon                                    ) const
{

    if ((!this->isDefined()) || (!aPolygon.isDefined()))
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return MultiPolygon::Polygon(*this).unionWith(MultiPolygon::Polygon(aPolygon)) ;

}

String                          Polygon::toString                           (   const   Object::Format&             aFormat,
                                                                                const   Integer&                    aPrecision                                  ) const
{

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    return implUPtr_->toString(aFormat, aPrecision) ;

}

void                            Polygon::applyTransformation                (   const   Transformation&             aTransformation                             )
{

    if (!aTransformation.isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Transformation") ;
    }

    if (!this->isDefined())
    {
        throw ostk::core::error::runtime::Undefined("Polygon") ;
    }

    implUPtr_->applyTransformation(aTransformation) ;

}

Polygon                         Polygon::Undefined                          ( )
{
    return { Array<Point>::Empty() } ;
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

}
}
}
}
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


#include <boost/assert.hpp>
#include <cmath>
#include <limits>
#include <utility>

#include "mh_moves/mh_moves.hpp"
#include "mh_moves/mh_move_weights.hpp"
#include "sampling/simple.hpp"
#include "mh_moves/utility.hpp"
#include "tools/debug.hpp"

namespace biggles {

namespace mh_moves
{

float extend_at_beginning_(const boost::shared_ptr<const track>& track_to_extend,
    const time_stamp& final_time_stamp, const clutter_t& clutter,
    std::deque<observation>& new_track_obs)
{
    float log_prob = 0.f;
    observation last_obs = track_to_extend->observations().front();
    new_track_obs.clear();
    weights_t weights;
    for (time_stamp ts = track_to_extend->first_time_stamp() -1 ; ts >= final_time_stamp; --ts) {
        float radius = float(t(last_obs) - ts) * detail::speed_of_light_;
        std::deque<observation> candidates = clutter.locate_near(ts, last_obs, radius);

        weights.assign(candidates.size(), 0.f);
        std_normal_weight obs_weight(last_obs);
        float noobs_weight = obs_weight.sigmas(2, ts);
        std::transform(candidates.begin(), candidates.end(), weights.begin(), obs_weight);
        candidates.push_back(observation());
        weights.push_back(noobs_weight);
        float obs_prob = 1.f;
        observation new_obs =
            sampling::weighted_choice(candidates.begin(), weights.begin(), weights.end(), obs_prob);
        if (not noobs(new_obs)) {
            new_track_obs.push_back(new_obs);
            last_obs = new_obs;
        }
        log_prob += obs_prob;
    }
    return log_prob;
}
float extend_at_end_(const boost::shared_ptr<const track>& track_to_extend,
    const time_stamp& final_time_stamp, const clutter_t& clutter,
    std::deque<observation>& new_track_obs)
{
    float log_prob = 0.f;
    observation last_obs = track_to_extend->observations().back();
    new_track_obs.clear();
    weights_t weights;
    for (time_stamp ts = track_to_extend->last_time_stamp(); ts < final_time_stamp; ++ts) {
        float radius = float(ts - t(last_obs)) * detail::speed_of_light_;
        std::deque<observation> candidates = clutter.locate_near(ts, last_obs, radius);
        weights.assign(candidates.size(), 0.f);
        std_normal_weight obs_weight(last_obs);
        float noobs_weight = obs_weight.sigmas(2, ts);
        std::transform(candidates.begin(), candidates.end(), weights.begin(), obs_weight);
        candidates.push_back(observation());
        weights.push_back(noobs_weight);
        float obs_prob = 1.f;
        observation new_obs =
            sampling::weighted_choice(candidates.begin(), weights.begin(), weights.end(), obs_prob);
        if (not noobs(new_obs)) {
            new_track_obs.push_back(new_obs);
            last_obs = new_obs;
        }
        log_prob += obs_prob;
    }
    return log_prob;
}

std::pair<bool, float> new_last_time_stamp_(const boost::shared_ptr<const track>& track_to_extend,
    const partition_ptr_t& original, time_stamp& new_last_time_stamp)
{
    new_last_time_stamp = track_to_extend->last_time_stamp();
    time_stamp max_time_step = original->last_time_stamp() - new_last_time_stamp;
    if (max_time_step == 0) {
        return std::make_pair(false, 0.f); // nothing to extend at the end
    }
    new_last_time_stamp += sampling::uniform_int(0, max_time_step) + 1;
    BOOST_ASSERT(new_last_time_stamp <= original->last_time_stamp());
    BOOST_ASSERT(new_last_time_stamp > track_to_extend->last_time_stamp());
    return std::make_pair(true, -logf(max_time_step));
}

bool extend(const partition_ptr_t& start_partition_ptr, partition_ptr_t& end_partition_ptr, float& proposal_density_ratio) {

    //partition original(start_partition);
    const clutter_t& clutter(start_partition_ptr->clutter());
    const track_collection& tracks(start_partition_ptr->tracks());
    if (tracks.size() == 0)
        return false;
    capability_recorder_ptr cap_rec_ptr = start_partition_ptr->get_capability_recorder();
    BOOST_ASSERT(cap_rec_ptr.get() not_eq 0); // there actually is something assigned
    //const track_collection::track_const_ptr_set& extendible_tracks(cap_rec_ptr->extendible_tracks());

    float log_track_prob = 1.f;
    shared_const_track_ptr track_to_extend;
    extend_weight_t extend_weight(start_partition_ptr->duration());

    if (not select_track(extend_weight, tracks, track_to_extend, log_track_prob)) {
        return false;
    }

    // front or back? p=0.5 doesn't need to be considered if(!) it is also ignored in reduce
    // if the following is false, then the track must be extendible at the end
    float log_front_or_end_prob = 0.f;
    bool extend_at_front = start_partition_ptr->first_time_stamp() < track_to_extend->first_time_stamp();
    BOOST_ASSERT(extend_at_front or start_partition_ptr->last_time_stamp() > track_to_extend->last_time_stamp());
    if (extend_at_front and start_partition_ptr->last_time_stamp() > track_to_extend->last_time_stamp()) {
        extend_at_front = sampling::uniform_int(0, 2) == 1;
        log_front_or_end_prob = -std::log(2.f);
    }

    time_stamp extension_last_time_stamp = track_to_extend->first_time_stamp();
    time_stamp extension_first_time_stamp = track_to_extend->last_time_stamp();
    float log_time_stamp_prob(1.f);
    float log_new_obs_prob(1.f);
    std::deque<observation> new_track_obs;
    if (not extend_at_front) {
        std::pair<bool, float> result = new_last_time_stamp_(track_to_extend, start_partition_ptr, extension_last_time_stamp);
        if (not result.first)
            return false; // nothing to extend at the end
        log_time_stamp_prob = result.second;
        log_new_obs_prob = extend_at_end_(track_to_extend, extension_last_time_stamp, clutter, new_track_obs);
    }
    else {
        if (start_partition_ptr->first_time_stamp() == track_to_extend->first_time_stamp())
            return false; // nothing to extend at the beginning
        extension_first_time_stamp = sampling::uniform_int(start_partition_ptr->first_time_stamp(), track_to_extend->first_time_stamp());
        log_time_stamp_prob = -logf(track_to_extend->first_time_stamp() - start_partition_ptr->first_time_stamp());
        log_new_obs_prob = extend_at_beginning_(track_to_extend, extension_first_time_stamp, clutter, new_track_obs);
    }
    const float new_dynamic_drag = 1.0f;
    boost::shared_ptr<track> track_extension(new track(extension_first_time_stamp, extension_last_time_stamp,
        new_track_obs.begin(), new_track_obs.end(), new_dynamic_drag));
    boost::shared_ptr<track> new_track(new track(*track_extension, *track_to_extend, track_to_extend->dynamic_drag()));

    // create copies of the partition clutter and tracks
    clutter_ptr new_clutter(new clutter_t(clutter));
    boost::shared_ptr<track_collection> new_tracks(new track_collection(tracks));

    cap_rec_ptr->add_insert_track(new_track);
    cap_rec_ptr->add_erase_track(track_to_extend);
    cap_rec_ptr->set_editing_finished();

    // replace old track
    new_tracks->replace(track_to_extend, new_track);

    // remove obs from clutter
    BOOST_FOREACH(const observation& o, new_track_obs) {
        new_clutter->erase(o);
    }

    // update partition
    end_partition_ptr = partition_ptr_t(
        new partition(start_partition_ptr->pool(), new_tracks, new_clutter, start_partition_ptr->expansion())
        );

    // reduce prob = prob(selecting correct track) * prob(sample the correct time point)
    //float reduce_log_prob = -logf(new_tracks->size());
    float reduce_log_prob = get_track_log_prob(reduce_weight_t(), *new_tracks, new_track);
    observation_collection::const_iterator obs_iter = new_track->begin();
    if (extend_at_front) {
        size_t num_obs = new_track->size();
        std::advance(obs_iter, num_obs -2); // the second last observation (we want to keep that)
        reduce_log_prob += -logf(t(*obs_iter) - new_track->first_time_stamp());
    } else {
        std::advance(obs_iter, 1); // the second observation (we want to keep that)
        reduce_log_prob += -logf(new_track->last_time_stamp() - t(*obs_iter)-1);
    }
    reduce_log_prob +=  -std::log(2.f);

    // extend prob == prob of sampling correct observation
    float extend_log_prob = log_track_prob + log_time_stamp_prob + log_new_obs_prob + log_front_or_end_prob;

    proposal_density_ratio = reduce_log_prob - extend_log_prob;
    return true;

}

}

}


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

#include <boost/property_tree/ptree.hpp>
#include "clotho/utility/clotho_strings.hpp"

template < class RealType >
struct bernoulli_distribution_parameter {

    typedef RealType    real_type;

    real_type           m_p;

    bernoulli_distribution_parameter( boost::property_tree::ptree & config ) :
        m_p( 0.5 )
    {
        m_p = config.get< real_type >( P_K, m_p );
        config.put( P_K, m_p );
    }

    bernoulli_distribution_parameter( real_type p = 0.5 ) :
        m_p( p )
    {}

    virtual ~bernoulli_distribution_parameter() {}
};
#endif  // BERNOULLI_DISTRIBUTION_PARAMETER_HPP_


#include <boost/test/unit_test.hpp>
#include <moreorg/ModelPool.hpp>
#include <moreorg/ModelPoolIterator.hpp>
#include <moreorg/Algebra.hpp>
#include <moreorg/vocabularies/OM.hpp>

using namespace moreorg;
using namespace moreorg::vocabulary;

BOOST_AUTO_TEST_SUITE(model_pool)

BOOST_AUTO_TEST_CASE(negative_positive_null)
{
    ModelPoolDelta a;
    a["a"] = -1;
    a["b"] = 0;
    a["c"] = 0;
    BOOST_REQUIRE_MESSAGE(a.isNegative(), "Model pool is negative");
    BOOST_REQUIRE_MESSAGE(!a.isPositive(), "Model pool is not positive");
    BOOST_REQUIRE_MESSAGE(!a.isNull(), "Model pool is not null");

    a["a"] = 2;
    BOOST_REQUIRE_MESSAGE(!a.isNegative(), "Model pool is not negative");
    BOOST_REQUIRE_MESSAGE(a.isPositive(), "Model pool is positive");
    BOOST_REQUIRE_MESSAGE(!a.isNull(), "Model pool is not null");

    a["a"] = 0;
    BOOST_REQUIRE_MESSAGE(!a.isNegative(), "Model pool is not negative");
    BOOST_REQUIRE_MESSAGE(!a.isPositive(), "Model pool is not positive");
    BOOST_REQUIRE_MESSAGE(a.isNull(), "Model pool is null");
}

BOOST_AUTO_TEST_CASE(equals)
{
    ModelPool a;
    a["a"] = 1;
    a["b"] = 10;
    a["c"] = 3;

    ModelPool b;
    b["b"] = a["b"];
    b["a"] = a["a"];
    b["c"] = a["c"];

    BOOST_REQUIRE_MESSAGE(a == b, "ModelPools are the same");
}

BOOST_AUTO_TEST_CASE(iterator)
{
    ModelPool from;
    ModelPool to;
    ModelPool step;

    from["a"] = 1;
    to["a"] = 10;
    step["a"] = 1;

    {
        ModelPoolIterator iterator(from, to, step);
        int i = 0;
        while(iterator.next())
        {
            ++i;
            ModelPool pool = iterator.current();
            BOOST_TEST_MESSAGE(pool.toString());
        }
        BOOST_REQUIRE_MESSAGE(i == 10, "Iterations expected: 10 but counted '" << i << "'");
    }

    step["a"] = 2;
    {
        ModelPoolIterator iterator(from, to, step);
        int i = 0;
        while(iterator.next())
        {
            ++i;
            ModelPool pool = iterator.current();
            BOOST_TEST_MESSAGE(pool.toString());
        }
        // 1 3 5 7 9 10
        BOOST_REQUIRE_MESSAGE(i == 6, "Iterations expected: 6, but counted '" << i << "'");
    }

    to["a"] = 3;
    from["b"] = 1;
    to["b"] = 3;
    step["a"] = 1;
    step["b"] = 1;
    {
        ModelPoolIterator iterator(from, to, step);
        int i = 0;
        while(iterator.next())
        {
            ++i;
            ModelPool pool = iterator.current();
            BOOST_TEST_MESSAGE(pool.toString());
        }
        // 1 3 5 7 9 10
    //    BOOST_REQUIRE_MESSAGE(i == 6, "Iterations expected: 6, but counted '" << i << "'");
    }

}

BOOST_AUTO_TEST_CASE(apply_upper_bound)
{
    using namespace owlapi::vocabulary;
    using namespace owlapi::model;

    IRI sherpa = OM::resolve("Sherpa");
    IRI crex = OM::resolve("CREX");
    IRI payload = OM::resolve("Payload");
    IRI payloadCamera = OM::resolve("PayloadCamera");

    // upperBound
    ModelPool modelPool;
    modelPool[sherpa] = 1;
    modelPool[crex] = 1;

    ModelCombinationSet combinations;
    {
        IRIList combination;
        combination.push_back(sherpa);
        combinations.insert(combination);
    }
    {
        IRIList combination;
        combination.push_back(sherpa);
        combination.push_back(crex);
        combinations.insert(combination);
    }
    {
        IRIList combination;
        combination.push_back(crex);
        combination.push_back(crex);
        combination.push_back(crex);
        combinations.insert(combination);
    }

    {
        ModelCombinationSet boundedSet = modelPool.applyUpperBound(combinations, modelPool);
        BOOST_REQUIRE_MESSAGE(boundedSet.size() == 2, "BoundedSet: expected size: 2 was " << boundedSet.size() << ": " << OrganizationModel::toString(boundedSet) );
    }

    {
        // empty upper bound
        ModelPool pool;
        ModelCombinationSet boundedSet = modelPool.applyUpperBound(combinations,
                pool);
        BOOST_REQUIRE_MESSAGE(boundedSet.size() == 3, "BoundedSet: expected size: 3 was " << boundedSet.size() << ": " << OrganizationModel::toString(boundedSet) );
    }
}

BOOST_AUTO_TEST_SUITE_END()


/* Copyright (c) 2019 Kulpreet Singh
 *
 * 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.
 */

#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE Fixtures

#include <boost/assign/list_of.hpp>
#include <boost/test/unit_test.hpp>

#include "include/algos.hpp"
#include "include/degree_stats.hpp"
#include "include/vertex.hpp"

using namespace lncentrality;

namespace tt = boost::test_tools;

struct Graph {
  LitGraph g;

  Graph() {
    BOOST_TEST_MESSAGE("setup mass");
    readgraph(g, std::cin);
  }

  ~Graph() { BOOST_TEST_MESSAGE("teardown mass"); }
};

BOOST_FIXTURE_TEST_SUITE(Degrees, Graph)

BOOST_AUTO_TEST_CASE(degrees) {
  vector<Degree> expected_in = boost::assign::list_of(1)(1)(2);
  vector<Degree> expected_out = boost::assign::list_of(2)(1)(1);
  vector<Degree> expected_total = boost::assign::list_of(3)(2)(3);

  vector<Degree> in_degrees, out_degrees, degrees;
  get_degrees(in_degrees, out_degrees, degrees, g);
  BOOST_TEST(in_degrees == expected_in, tt::per_element());
  BOOST_TEST(out_degrees == expected_out, tt::per_element());
  BOOST_TEST(degrees == expected_total, tt::per_element());
}

BOOST_AUTO_TEST_SUITE_END()


//==============================================================================
//         Copyright 2003 - 2013   LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2013   LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#include <nt2/include/functions/tanpi.hpp>

#include <boost/dispatch/functor/meta/call.hpp>
#include <nt2/sdk/functor/meta/call.hpp>
#include <nt2/sdk/unit/tests/relation.hpp>
#include <nt2/sdk/unit/tests/type_expr.hpp>
#include <complex>
#include <nt2/sdk/complex/complex.hpp>
#include <nt2/sdk/unit/tests/ulp.hpp>
#include <nt2/sdk/unit/tests/basic.hpp>
#include <nt2/sdk/meta/as_integer.hpp>
#include <nt2/sdk/unit/module.hpp>
#include <boost/simd/sdk/config.hpp>

#include <nt2/include/constants/one.hpp>
#include <nt2/include/constants/pi.hpp>
#include <nt2/include/constants/zero.hpp>
#include <nt2/include/constants/inf.hpp>
#include <nt2/include/constants/minf.hpp>
#include <nt2/include/constants/nan.hpp>
#include <nt2/include/constants/i.hpp>

#include <nt2/include/functions/mul_i.hpp>
#include <nt2/include/functions/mul_minus_i.hpp>

NT2_TEST_CASE_TPL ( tanpi,  NT2_REAL_TYPES)
{

  using nt2::tanpi;
  using nt2::tag::tanpi_;
  typedef std::complex<T> cT;
  typedef typename nt2::meta::call<tanpi_(cT)>::type r_t;
  typedef typename nt2:: meta::as_complex<T>::type wished_r_t;

  // return type conformity test
  NT2_TEST_TYPE_IS(r_t, wished_r_t);

  // specific values tests
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(nt2::Inf<T>())), cT(nt2::Nan<T>()), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(nt2::Minf<T>())), cT(nt2::Nan<T>()), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(1, 1)),nt2::tan(nt2::Pi<T>()*cT(1.0, 1.0)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(1, 0.25)),nt2::tan(nt2::Pi<T>()*cT(1.0, 0.25)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(0.25, 1)),nt2::tan(nt2::Pi<T>()*cT(0.25, 1.0)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(0.25, 0.25)),nt2::tan(nt2::Pi<T>()*cT(0.25, 0.25)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(0, 1)),nt2::tan(nt2::Pi<T>()*cT(0.0, 1.0)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(0, 0.25)),nt2::tan(nt2::Pi<T>()*cT(0.0, 0.25)), 2);
  NT2_TEST_ULP_EQUAL(nt2::tanpi(cT(0.25, 0)),nt2::tan(nt2::Pi<T>()*cT(0.25, 0.0)), 2);

  const int N = 20;
  cT inputs[N] =
    { cT(nt2::Zero<T>(),nt2::Zero<T>()),cT(nt2::Inf<T>(),nt2::Zero<T>()),cT(nt2::Minf<T>(),nt2::Zero<T>()),cT(nt2::Nan<T>(),nt2::Zero<T>()),
      cT(nt2::Zero<T>(),nt2::Inf<T>()), cT(nt2::Inf<T>(),nt2::Inf<T>()), cT(nt2::Minf<T>(),nt2::Inf<T>()), cT(nt2::Nan<T>(),nt2::Inf<T>()),
      cT(nt2::Zero<T>(),nt2::Minf<T>()),cT(nt2::Inf<T>(),nt2::Minf<T>()),cT(nt2::Minf<T>(),nt2::Minf<T>()),cT(nt2::Nan<T>(),nt2::Minf<T>()),
      cT(nt2::Zero<T>(),nt2::Nan<T>()), cT(nt2::Inf<T>(),nt2::Nan<T>()), cT(nt2::Minf<T>(),nt2::Nan<T>()), cT(nt2::Nan<T>(),nt2::Nan<T>()),
      cT(nt2::Zero<T>(),nt2::One<T>()), cT(nt2::Inf<T>(),nt2::One<T>()), cT(nt2::Minf<T>(),nt2::One<T>()), cT(nt2::Nan<T>(),nt2::One<T>()),
    };

  for(int i=0; i < N; i++)
   {
     std::cout << "-------------------" << std::endl;
     std::cout << "inputs  "<< inputs[i] << std::endl;
     NT2_TEST_ULP_EQUAL(nt2::tanpi(-inputs[i]), -nt2::tanpi(inputs[i]), 3);
     NT2_TEST_ULP_EQUAL(nt2::tanpi(inputs[i]), nt2::mul_minus_i(nt2::tanh(nt2::mul_i(nt2::multiplies(nt2::Pi<T>(), inputs[i])))), 3);
     std::cout << "=================== " << std::endl;
   }

 } // end of test for floating_


 #include "./../../header/nuutila.h"
#include "./../../header/visualize.h"
//STL
#include <iostream>
#include <vector>
#include <string>
#include <fstream>
#include <sstream>
#include <iterator>
//Boost
#include <boost/config.hpp>
#include <boost/graph/strong_components.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/graphviz.hpp>
#include <boost/graph/graph_utility.hpp>
#include <boost/graph/adjacency_list_io.hpp>
#include <boost/graph/property_iter_range.hpp>
#include <boost/graph/copy.hpp>
#include <boost/property_map/property_map.hpp>

void Nuutila::print_graph()
{
    GraphComponent::print_graph_file(n);
}

void Nuutila::print_sccs(UtilityStructs::StorageItems &s)
{
    std::vector<int> root = s.auxilary;
    Visualize vis;
    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp1;
    std::pair<vertex_iter, vertex_iter> vp_inner;
    for (vp1 = vertices(n); vp1.first != vp1.second; ++vp1.first)
    {
        Vertex_t v = *vp1.first;
        if (id[v] == root[id[v]])
        {
            vis.printLine("");
            std::stringstream buffer ;
            buffer << std::setw(40) << std::left << "The Strongly Connected Component is" ;
            buffer << ": ";

            int count_component = 0;
            for (vp_inner = vertices(n); vp_inner.first != vp_inner.second; ++vp_inner.first)
            {
                Vertex_t w = *vp_inner.first;
                if (root[id[w]] == id[v])
                {
                    buffer << id[w]+1 << " " ;
                    count_component++;
                }
            }
            vis.printLine(buffer.str());
            std::stringstream buffer2;
           // vis.printLine("");
            buffer2 << std::setw(40) << std::left << "Number of elements in the component is" ;
            buffer2 << ": "<< count_component ;
            vis.printLine(buffer2.str());
        }
    }
            std::stringstream buffer3;
        vis.printProgramBottom();
        buffer3 << std::setw(40) << std::left <<"Completion Time" ;
        buffer3 << ": " << s.duration << " miliseconds" ;
        std::stringstream buffer4;
        buffer4 << std::setw(40)<< std::left << "Total Execution Size";
        buffer4 << ": " << s.total_bytes / 1000.f << " KBs";
        vis.printLine(buffer3.str());
        vis.printLine(buffer4.str());
}
UtilityStructs::StorageItems Nuutila::ApplySCC_Original()
{
    float ms_duration;
    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    std::vector<Vertex_t> Points;
    int Counter = 0;
    int stackCount = 0;
    int sizeOfGraph = num_vertices(n);
    std::vector<bool> isComponent(sizeOfGraph, false);
    std::vector<int> visited(sizeOfGraph, 999999);
    std::vector<int> root(sizeOfGraph, 666);
    //Timer
    //   std::cout << "\nThe Nuutila Version of the SCC Algorithms is processing the graph" << std::endl;
    {
        UtilityStructs::Timer timer;
        for (vp = vertices(n); vp.first != vp.second; vp.first++)
        {
            Vertex_t v = *vp.first;
            if (visited[id[v]] == 999999)
            {
                Visit(v, Points, root, visited, isComponent, Counter,stackCount);
            }
        }
        ms_duration = timer.stop();
    }
    size_t total_bytes = 0;
    total_bytes += sizeof(root[0]) * root.size();
    total_bytes += sizeof(visited[0]) * visited.size();
    total_bytes += sizeof(isComponent[0]) * isComponent.size();
    total_bytes += sizeof(Points[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(n);
    s.edgeCount = num_edges(n);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    s.auxilary = root;
    return s;

}

void Nuutila::Visit(Vertex_t &v, std::vector<Vertex_t> &Points, std::vector<int> &root, std::vector<int> &visited, std::vector<bool> &isComponent, int &Counter,int &stackCount)
{
    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    visited[id[v]] = Counter;
    Counter++;
    root[id[v]] = id[v];
    isComponent[id[v]] = false;
    Points.push_back(v);
    stackCount++;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator o_i, o_o;
    for (boost::tie(o_i, o_o) = out_edges(v, n); o_i != o_o; ++o_i)
    {
        Edge e = *o_i;
        Vertex_t w = target(e, n);
        if (visited[id[w]] == 999999)
        {
            Visit(w, Points, root, visited, isComponent, Counter,stackCount);
        }
        if (!isComponent[id[w]])
        {
            if (visited[root[id[v]]] > visited[root[id[w]]])
            {
                root[id[v]] = root[id[w]];
            }
        }
    }
    if (root[id[v]] == id[v])
    {
        Vertex_t w;
        do
        {
            w = Points.back();
            Points.pop_back();
            isComponent[id[w]] = true;

        } while (v != w);
    }
}

UtilityStructs::StorageItems Nuutila::ApplySCC_v1()
{
    float ms_duration;
    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    std::vector<Vertex_t> Points;
    int Counter = 0;
    int stackCount = 0;
    int sizeOfGraph = num_vertices(n);
    std::vector<bool> isComponent(sizeOfGraph, false);
    std::vector<int> visited(sizeOfGraph, 999999);
    std::vector<int> root(sizeOfGraph, -1);
    //    std::cout << "\nThe Nuutila First Modified Version of the SCC Algorithms is processing the graph" << std::endl;
    {
        UtilityStructs::Timer timer;
        for (vp = vertices(n); vp.first != vp.second; vp.first++)
        {
            Vertex_t v = *vp.first;
            if (visited[id[v]] == 999999)
            {
                Visit_v1(v, Points, root, visited, isComponent, Counter,stackCount);
            }
        }
        ms_duration = timer.stop();
    }
    //Storage info collection
    size_t total_bytes = 0;
    total_bytes += sizeof(root[0]) * root.size();
    total_bytes += sizeof(visited[0]) * visited.size();
    total_bytes += sizeof(isComponent[0]) * isComponent.size();
    total_bytes += sizeof(Points[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(n);
    s.edgeCount = num_edges(n);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    s.auxilary = root;
    return s;
}

UtilityStructs::StorageItems Nuutila::ApplySCC_v2()
{
    float ms_duration;
    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    typedef boost::graph_traits<theGraph>::vertex_iterator vertex_iter;
    std::pair<vertex_iter, vertex_iter> vp;
    std::vector<int> Points;
    Points.push_back(0);
    int Counter = 0;
    int stackCount = 0;
    int sizeOfGraph = num_vertices(n);
    std::vector<bool> isComponent(sizeOfGraph + 1, false);
    std::vector<int> visited(sizeOfGraph + 1, 999999);
    std::vector<int> root(sizeOfGraph, -1);
    visited[0] = -999;
    //   std::cout << "\nThe Nuutila Second Modified Version of the SCC Algorithms is processing the graph" << std::endl;
    {
        UtilityStructs::Timer timer;
        for (vp = vertices(n); vp.first != vp.second; vp.first++)
        {
            Vertex_t v = *vp.first;
            if (visited[id[v] + 1] == 999999)
            {
                Visit_v2(v, Points, root, visited, isComponent, Counter,stackCount);
            }
        }
        ms_duration = timer.stop();
        //Storage info collection
    }
    size_t total_bytes = 0;
    total_bytes += sizeof(root[0]) * root.size();
    total_bytes += sizeof(visited[0]) * visited.size();
    total_bytes += sizeof(isComponent[0]) * isComponent.size();
    total_bytes += sizeof(Points[0]) * stackCount;
    UtilityStructs::StorageItems s;
    s.vertexCount = num_vertices(n);
    s.edgeCount = num_edges(n);
    s.duration = ms_duration;
    s.total_bytes = total_bytes;
    s.auxilary = root;
    return s;

    // print_sccs(root);
}

void Nuutila::Visit_v1(Vertex_t &v, std::vector<Vertex_t> &Points, std::vector<int> &root, std::vector<int> &visited, std::vector<bool> &isComponent, int &Counter,int &stackCount)
{

    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    visited[id[v]] = Counter;
    Counter++;
    root[id[v]] = id[v];
    isComponent[id[v]] = false;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator hoho, haha;
    for (boost::tie(hoho, haha) = out_edges(v, n); hoho != haha; ++hoho)
    {
        Edge e = *hoho;
        Vertex_t w = target(e, n);
        if (visited[id[w]] == 999999)
        {
            Visit_v1(w, Points, root, visited, isComponent, Counter,stackCount);
        }
        if (!isComponent[id[w]])
        {
            if (visited[root[id[v]]] > visited[root[id[w]]])
            {
                root[id[v]] = root[id[w]];
            }
            else
            {
                root[id[w]] = root[id[v]];
            }
        }
    }
    if (root[id[v]] == id[v])
    {
        isComponent[id[v]] = true;
        if (!Points.empty())
        {
            Vertex_t w = Points.back();
            while (visited[id[w]] > visited[id[v]])
            {
                Points.pop_back();
                isComponent[id[w]] = true;
                if (Points.empty())
                {
                    break;
                }
                else
                {
                    w = Points.back();
                }
            }
        }
    }
    else
    {
        Points.push_back(v);
        stackCount++;
    }
}

void Nuutila::Visit_v2(Vertex_t &v, std::vector<int> &Points, std::vector<int> &root, std::vector<int> &visited, std::vector<bool> &isComponent, int &Counter,int &stackCount)
{
    v_p id = get(&VertexProperty::index, n);
    typedef boost::graph_traits<theGraph>::vertex_descriptor Vertex_t;
    visited[id[v] + 1] = Counter;
    Counter++;
    root[id[v]] = id[v];
    isComponent[id[v] + 1] = false;
    typedef boost::graph_traits<theGraph>::edge_descriptor Edge;
    boost::graph_traits<theGraph>::out_edge_iterator hoho, haha;
    for (boost::tie(hoho, haha) = out_edges(v, n); hoho != haha; ++hoho)
    {
        Edge e = *hoho;
        Vertex_t w = target(e, n);
        if (visited[id[w] + 1] == 999999)
        {
            Visit_v2(w, Points, root, visited, isComponent, Counter,stackCount);
        }
        if (!isComponent[root[id[w]] + 1])
        {
            if (visited[root[id[v]] + 1] > visited[root[id[w]] + 1])
            {
                root[id[v]] = root[id[w]];
            }
        }
    }
    if (root[id[v]] == id[v])
    {
        int w = Points.back();
        if (visited[w] >= visited[id[v] + 1])
        {
            do
            {
                Points.pop_back();
                isComponent[w] = true;
                w = Points.back();
            } while (visited[w] >= visited[id[v] + 1]);
        }
        else
        {
            isComponent[id[v] + 1] = true;
        }
    }
    else if (std::find(Points.begin(), Points.end(), root[id[v]]) == Points.end())
    {
        Points.push_back(root[id[v]]);
        stackCount++;
    }
}


//Copyright (c) 2013 Singapore-MIT Alliance for Research and Technology
//Licensed under the terms of the MIT License, as described in the file:
//   license.txt   (http://opensource.org/licenses/MIT)

#pragma once

#include <boost/unordered_set.hpp>

#include "metrics/Length.hpp"
#include "geospatial/network/WayPoint.hpp"

namespace sim_mob
{

//Forward declarations
class Agent;
class Entity;
class Lane;
class Point;
class RoadSegment;

namespace spatial
{

/**
 * @param agent1
 * @param agent2
 * @return the squared euclidean distance between agent1 and agent2.
 */
double distance(const sim_mob::Agent *agent1, const sim_mob::Agent *agent2);

/**
 * Find the agent in the collection of agents that is closest to the given agent.
 *
 * @param agent the agent to be searched around
 * @param agents the collection of agents
 *
 * @return the agent in the collection of agents that is closest to the given agent
 */
const sim_mob::Agent* nearest_agent(const sim_mob::Agent *agent, const boost::unordered_set<const sim_mob::Entity *> &agents);

/**
 * Calculates the sum of the width of lane/turning path and the width of the adjacent lanes/turning paths on the
 * left and right of the given lane/turning path.
 * If there is no lane on the left, add 0.3 metre. Similarly if there is no lane on the right, add 0.3 metre.
 *
 * @param wayPoint holds the lane or the turning path
 *
 * @return the sum of the width of lane/turning path and the width of the adjacent lanes/turning paths
 */
double getAdjacentPathWidth(const sim_mob::WayPoint &wayPoint);

// Return true if <point> is between <p1> and <p2>, even if <point> is not co-linear
// with <p1> and <p2>.
bool isInBetween(const sim_mob::Point &point, const sim_mob::Point &p1, const sim_mob::Point &p2);

// Adjust <p1> and <p2> so that <p2> is <distanceInFront> from <position> and
// <p1> is <distanceBehind> from <position>, while retaining the slope of the line
// from <p1> to <p2>.
void adjust(sim_mob::Point &p1, sim_mob::Point &p2, const sim_mob::Point &position, double distanceInFront, double distanceBehind);

}
}