//          Copyright Gavin Band 2008 - 2012.
// Distributed under the Boost Software License, Version 1.0.
//    (See accompanying file LICENSE_1_0.txt or copy at
//          http://www.boost.org/LICENSE_1_0.txt)

#include <string>
#include <boost/function.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/thread.hpp>
#include <Eigen/Core>
#include "genfile/SNPDataSourceProcessor.hpp"
#include "genfile/SNPDataSourceChain.hpp"
#include "genfile/SampleFilteringSNPDataSource.hpp"
#include "genfile/SampleMappingSNPDataSource.hpp"
#include "genfile/wildcard.hpp"
#include "genfile/ToGP.hpp"
#include "statfile/BuiltInTypeStatSink.hpp"
#include "integration/NewtonRaphson.hpp"
#include "integration/Derivative.hpp"
#include "db/Connection.hpp"
#include "db/SQLStatement.hpp"
#include "db/Error.hpp"
#include "components/HaplotypeFrequencyComponent/FlatTableDBOutputter.hpp"
#include "components/HaplotypeFrequencyComponent/HaplotypeFrequencyComponent.hpp"

// #define DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT 1
struct HaplotypeFrequencyLogLikelihood ;

void HaplotypeFrequencyComponent::declare_options( appcontext::OptionProcessor& options ) {
	options.declare_group( "LD computation options" ) ;
	options[ "-compute-ld-with" ]
		.set_description( "Compute LD pairwise metrics between the main dataset and SNPs." )
		.set_takes_values(2) ;
	options[ "-old" ]
		.set_description( "Specify file to write LD metrics to" )
		.set_takes_single_value() ;
	options[ "-max-ld-distance" ]
		.set_description( "Maximum physical distance between SNPs, above which LD will not be computed. "
			"A value of zero indicates LD between all SNPs will be computed. "
			"A plain number indicates distance in base pairs, or you add a Mb or kb suffix to specify the "
			"distance in megabases or kilobases if desired." )
		.set_takes_single_value()
		.set_default_value( "0" ) ;
	options[ "-min-r2" ]
		.set_description( "Minimum squared correlation between variants.  LD results for pairs of variants with "
			"lower than this squared correlation will not be output." )
		.set_takes_single_value()
		.set_default_value( "0" ) ;
	options[ "-prior-ld-weight" ]
		.set_description( "Weight w to place on shrinkage prior in computation of pairwise LD. "
			"This is interpreted as adding dummy observations of w/4 for each of the four possible haplotypes, "
			"or equivalently, to placing a Dirichlet(w/4, w/4, w/4, w/4) prior on the vector of haplotype frequencies." )
		.set_takes_single_value()
		.set_default_value( 1.0 ) ;
	options.option_implies_option( "-compute-ld-with", "-old" ) ;
	options.option_implies_option( "-prior-ld-weight", "-compute-ld-with" ) ;
	options.option_implies_option( "-max-ld-distance", "-compute-ld-with" ) ;
	options.option_implies_option( "-min-r2", "-compute-ld-with" ) ;
}

namespace {
	uint64_t parse_physical_distance( std::string distance ) {
		std::size_t number_part_length = std::string::npos ;
		uint64_t multiplier = 1 ;
		// allowable suffixes are mb, kb, bp.

		if( distance.size() == 0 ) {
			throw genfile::BadArgumentError( "parse_physical_distance()", "distance=\"" + distance + "\"" ) ;
		}

		if( distance[ distance.size() - 1 ] != 'b' && distance[ distance.size() - 1 ] != 'b' ) {
			// no suffix.
			number_part_length = distance.size() ;
			multiplier = 1 ;
		}
		else {
			if( distance.size() < 3 ) {
				throw genfile::BadArgumentError( "parse_physical_distance()", "distance=\"" + distance + "\"" ) ;
			}
			number_part_length = distance.size() - 2 ;
			std::string suffix = genfile::string_utils::to_lower( distance.substr( distance.size() - 2, 2 ) ) ;
			if( suffix == "bp" ) {
				multiplier = 1 ;
			} else if( suffix == "kb" ) {
				multiplier = 1000 ;
			} else if( suffix == "mb" ) {
				multiplier = 1000000 ;
			} else {
				throw genfile::BadArgumentError( "parse_physical_distance()", "distance=\"" + distance + "\"" ) ;
			}
		}

		uint64_t const result = genfile::string_utils::to_repr< double >( distance.substr( 0, number_part_length ) ) * multiplier ;
		std::cerr << "Parsed distance \"" + distance + "\" as " + genfile::string_utils::to_string( result ) + " base pairs.\n" ;
		return result ;
	}
}

namespace {
	genfile::SampleStratification compute_stratification( genfile::CohortIndividualSource const& samples, std::string const& variable ) {
		genfile::SampleStratification result ;
		genfile::CohortIndividualSource::ColumnSpec const spec = samples.get_column_spec() ;
		if( !spec[ variable ].is_discrete() ) {
			throw genfile::BadArgumentError( "void impl::compute_strata()", "variable=\"" + variable + "\"" ) ;
		}

		for( std::size_t i = 0; i < samples.get_number_of_individuals(); ++i ) {
			genfile::VariantEntry const& entry = samples.get_entry( i, variable ) ;
			if( !entry.is_missing() ) {
				result.add_sample( variable + "=" + entry.as< std::string >(), i ) ;
			}
		}

		return result ;
	}
}

HaplotypeFrequencyComponent::UniquePtr HaplotypeFrequencyComponent::create(
	genfile::CohortIndividualSource const& source_samples,
	std::string const& source_sample_id_column,
	genfile::CohortIndividualSource::UniquePtr samples,
	std::string const& ld_sample_id_column,
	genfile::SNPDataSource::UniquePtr source,
	appcontext::OptionProcessor const& options,
	appcontext::UIContext& ui_context
) {
	source.reset(
		new genfile::SampleMappingSNPDataSource(
			source_samples,
			source_sample_id_column,
			*samples,
			ld_sample_id_column,
			source
		)
	) ;

	HaplotypeFrequencyComponent::UniquePtr result ;

#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
	std::cerr << "LD SNP samples: " << source->number_of_samples() << " (matched from " << source->get_parent_source().number_of_samples() << " in raw data).\n" ;
#endif
	result.reset(
		new HaplotypeFrequencyComponent(
			source,
			ui_context
		)
	) ;

	result->set_max_distance( parse_physical_distance( options.get< std::string >( "-max-ld-distance" ))) ;
	result->set_min_r2( options.get< double >( "-min-r2" )) ;
	{
		double const w = options.get< double >( "-prior-ld-weight" ) ;
		Eigen::MatrixXd prior(2,2) ;
		prior
			<< w/4, w/4,
			   w/4, w/4
		;
		result->set_prior( prior ) ;
	}

	using namespace genfile::string_utils ;
	std::string filename = options.get_value< std::string >( "-old" ) ;
	if( filename.size() >= 9 && filename.substr( 0, 9 ) == "sqlite://" ) {
		filename = filename.substr( 9, filename.size() ) ;
	}
	std::vector< std::string > outputFilenameElts = genfile::string_utils::split( filename, ":" ) ;
	assert( outputFilenameElts.size() > 0 ) ;
	if( outputFilenameElts.size() > 2 ) {
		throw genfile::BadArgumentError(
			"HaplotypeFrequencyComponent::create()",
			"-old \"" + options.get_value< std::string >( "-old" ) + "\"",
			"Value should be of the form [sqlite://]<filename>[:<tablename>]"
		) ;
	}

	haplotype_frequency_component::FlatTableDBOutputter::UniquePtr outputter
		= haplotype_frequency_component::FlatTableDBOutputter::create(
			outputFilenameElts[0],
			options.get_value< std::string >( "-analysis-name" ),
			"HaplotypeFrequencyComponent",
			options.get_values_as_map()
	) ;
	if( outputFilenameElts.size() == 2 ) {
		outputter->set_table_name( outputFilenameElts[1] ) ;
	}

	if( options.check( "-stratify" )) {
		result->set_stratification( compute_stratification( source_samples, options.get< std::string >( "-stratify" )) ) ;
	}

	result->send_results_to( outputter ) ;
	return result ;
}

HaplotypeFrequencyComponent::HaplotypeFrequencyComponent(
	genfile::SNPDataSource::UniquePtr source,
	appcontext::UIContext& ui_context
):
	m_source( source ),
	m_ui_context( ui_context ),
	m_threshhold( 0.9 ),
	m_max_distance( 200000 ),
	m_min_r2( 0.0 ),
	m_prior( Eigen::Matrix2d::Zero() )
{
}

void HaplotypeFrequencyComponent::set_max_distance( uint64_t distance ) {
	m_max_distance = distance ;
}

void HaplotypeFrequencyComponent::set_min_r2( double min_r2 ) {
	assert( min_r2 >= 0.0 ) ;
	m_min_r2 = min_r2 ;
}

void HaplotypeFrequencyComponent::set_prior( Eigen::Matrix2d const& matrix ) {
	assert( matrix.array().minCoeff() >= 0.0 ) ;
	m_prior = matrix ;
}

void HaplotypeFrequencyComponent::set_stratification( genfile::SampleStratification stratification ) {
	m_stratification = stratification ;
}

void HaplotypeFrequencyComponent::begin_processing_snps( std::size_t number_of_samples, genfile::SNPDataSource::Metadata const& ) {
	assert( m_source->number_of_samples() == number_of_samples ) ;
}

void HaplotypeFrequencyComponent::processed_snp(
	genfile::VariantIdentifyingData const& target_snp,
	genfile::VariantDataReader& target_data_reader
) {
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
	std::cerr << "Processing " << target_snp << "...\n" ;
#endif
	genfile::VariantIdentifyingData source_snp ;
	m_source->reset_to_start() ;
	while( m_source->get_snp_identifying_data( &source_snp )) {
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
		std::cerr << "Comparing to " << source_snp << "...\n" ;
#endif
		if(
			( m_max_distance == 0 )
			||
			(
				( source_snp.get_position().chromosome() == target_snp.get_position().chromosome() )
				&&
				( std::abs( int64_t( source_snp.get_position().position() ) - int64_t( target_snp.get_position().position() ) ) <= m_max_distance )
			)
		) {
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
			std::cerr << "Computing LD measures for " << source_snp << " : " << target_snp << "...\n" ;
#endif
			genfile::VariantDataReader::UniquePtr source_data_reader = m_source->read_variant_data() ;
			compute_ld_measures( source_snp, *source_data_reader, target_snp, target_data_reader ) ;
		}
		else {
			m_source->ignore_snp_probability_data() ;
		}
	}
}

namespace {
	struct CallSetter: public genfile::VariantDataReader::PerSampleSetter {
		enum { ePhased = 0x80000000 } ;

		CallSetter(
			std::vector< int >* result,
			std::vector< uint32_t >* ploidy
		):
			m_result( result ),
			m_ploidy( ploidy )
		{
			assert( result ) ;
		}

		void initialise( std::size_t nSamples, std::size_t nAlleles ) {
			if( nAlleles != 2 ) {
				throw genfile::BadArgumentError(
					"Callsetter::initialise()",
					"nAlleles=" + genfile::string_utils::to_string( nAlleles ),
					"Only biallelic variants are currently supported."
				) ;
			}
			m_result->clear() ; // ploidy 2
			m_result->resize( nSamples, -1 ) ;
			m_ploidy->clear() ;
			m_ploidy->resize( nSamples, 0 ) ;
		}

		bool set_sample( std::size_t n ) {
			m_sample_i = n ;
			return true ;
		}

		void set_number_of_entries(
			uint32_t ploidy, std::size_t n,
			genfile::OrderType const order_type,
			genfile::ValueType const value_type
		) {
			if( ploidy > 2 ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"ploidy=" + genfile::string_utils::to_string( ploidy ),
					"Only haploid or diploid samples are currently supported."
				) ;
			}
			if( value_type != genfile::eAlleleIndex ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"value_type=" + genfile::string_utils::to_string( value_type ),
					"Expected a hard-called genotype, consider using threshholded calls."
				) ;
			}
			if( order_type != genfile::ePerOrderedHaplotype && order_type != genfile::ePerUnorderedHaplotype ) {
				throw genfile::BadArgumentError(
					"Callsetter::set_number_of_entries()",
					"order_type=" + genfile::string_utils::to_string( order_type ),
					"Expected a hard-called genotype, consider using threshholded calls."
				) ;
			}
			assert( value_type == genfile::eAlleleIndex ) ;
			m_order_type = order_type ;
			uint32_t storedPloidy = ploidy ;
			if( m_order_type == genfile::ePerOrderedHaplotype ) {
				storedPloidy |= ePhased ;
			}
			(*m_ploidy)[m_sample_i] = storedPloidy ;
			(*m_result)[m_sample_i] = 0 ;
		}

		void set_value( std::size_t entry_i, genfile::MissingValue const value ) {
			(*m_result)[m_sample_i] = -1;
		}

		void set_value( std::size_t entry_i, Integer const value ) {
			// Only accumulate if not missing
			int& stored = (*m_result)[m_sample_i] ;
			if( stored != -1 ) {
				if( m_order_type == genfile::ePerUnorderedHaplotype ) {
					// Compute count of 2nd allele
					(*m_result)[m_sample_i] += value ;
				}
				else if( m_order_type == genfile::ePerOrderedHaplotype ) {
					// Put haplotype allele counts in seperate bits.
					(*m_result)[m_sample_i] += (value << entry_i) ;
				}
				else {
					assert(0) ;
				}
			}
		}

		void set_value( std::size_t entry_i, double const value ) {
			assert(0) ; // expecting GT field
		}

		void finalise() {
			// nothing to do
		}

	private:
		std::vector< int >* m_result ;
		std::vector< uint32_t >* m_ploidy ;
		std::size_t m_sample_i ;
		genfile::OrderType m_order_type ;
	} ;

	void tabulate_calls(
		std::vector< genfile::SampleRange > const& sample_set,
		std::vector< int > left_calls,
		std::vector< uint32_t > left_ploidy,
		std::vector< int > right_calls,
		std::vector< uint32_t > right_ploidy,
		Eigen::Matrix3d* diploid_table,
		Eigen::Matrix2d* haploid_table
	) {
		for( std::size_t range_i = 0; range_i < sample_set.size(); ++range_i ) {
			for( std::size_t j = sample_set[range_i].begin(); j < sample_set[range_i].end(); ++j ) {
				if( right_ploidy[j] == left_ploidy[j] && left_calls[j] != -1 && right_calls[j] != -1 ) {
					uint32_t const ploidy = left_ploidy[j] & 0xF;
					uint32_t const phased = left_ploidy[j] & CallSetter::ePhased ;
					switch( ploidy ) {
						case 0: break; // zeroploid, nothing to do
						case 1:
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT > 1
							std::cerr << "Setting haploid: " << left_calls[j] << ", " << right_calls[j] << ".\n" ;
#endif
							++((*haploid_table)( left_calls[j], right_calls[j] )) ;
							break ;
						case 2:
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT > 1
							std::cerr << "Setting diploid: " << left_calls[j] << ", " << right_calls[j] << ".\n" ;
#endif
							if( phased != 0 ) {
								++(*haploid_table)(
									(left_calls[j] & 0x1),
									(right_calls[j] & 0x1)
								) ;
								++(*haploid_table)(
									(left_calls[j] & 0x2) >> 1,
									(right_calls[j] & 0x2) >> 1
								) ;
							} else {
								++(*diploid_table)( left_calls[j], right_calls[j] ) ;
							}
							break ;
						default:
							assert(0) ;
					}
				}
			}
		}
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
		std::cerr << "Haploid table:\n" << *haploid_table << "\n" ;
		std::cerr << "Diploid table:\n" << *diploid_table << "\n" ;
#endif
	} ;

	struct DosageSetter: public genfile::VariantDataReader::PerSampleSetter {
		DosageSetter(
			Eigen::MatrixXd* result,
			Eigen::MatrixXd* nonmissingness,
			int column
		):
			m_result( result ),
			m_nonmissingness( nonmissingness ),
			m_order_type( genfile::eUnknownOrderType ),
			m_result_column( column )
		{
			assert( result ) ;
			assert( column >= 0 && column < result->cols() ) ;
		}

		void initialise( std::size_t nSamples, std::size_t nAlleles ) {
			assert( nAlleles == 2 ) ;
			assert( m_result->rows() == nSamples ) ;
			assert( m_nonmissingness->rows() == nSamples ) ;
		}

		bool set_sample( std::size_t n ) {
			m_sample_i = n ;
			m_order_type = genfile::eUnknownOrderType ;
			return true ;
		}

		void set_number_of_entries(
			uint32_t ploidy, std::size_t n,
			genfile::OrderType const order_type,
			genfile::ValueType const value_type
		) {
			assert( ploidy <= 2 ) ;
			assert( (order_type == genfile::ePerOrderedHaplotype || genfile::ePerUnorderedGenotype) && value_type == genfile::eProbability ) ;
			m_order_type = order_type ;
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT > 1
			std::cerr << m_order_type << ", " << m_sample_i << ", " << m_result_column << "\n" << std::flush ;
			std::cerr << m_result->rows() << " x " << m_result->cols() << ".\n" << std::flush ;
			std::cerr << m_nonmissingness->rows() << " x " << m_nonmissingness->cols() << ".\n" << std::flush ;
#endif
			(*m_result)(m_sample_i,m_result_column) = 0 ;
			(*m_nonmissingness)(m_sample_i,m_result_column) = 0 ;
		}

		void set_value( std::size_t entry_i, genfile::MissingValue const value ) {
			(*m_result)(m_sample_i, m_result_column) = 0 ;
			(*m_nonmissingness)(m_sample_i,m_result_column) = 0 ;
		}

		void set_value( std::size_t entry_i, double const value ) {
			// For biallelic variants, genotypes come in the order of
			// the number of B alleles, so this computes dosage:
			if( m_order_type == genfile::ePerOrderedHaplotype ) {
				// genotypes come in the order A, B (1st hap); A, B (2nd hap)
				// assumption is variant is biallelic.
				(*m_result)(m_sample_i, m_result_column) += (entry_i % 2) * value ;
			} else {
				// order type = genfile::ePerUnorderedGenotype
				// genotypes come in the order AA, AB, BB,
				(*m_result)(m_sample_i, m_result_column) += entry_i * value ;
			}
			(*m_nonmissingness)(m_sample_i,m_result_column) = 1 ;
		}

		void set_value( std::size_t entry_i, Integer const value ) {
			assert(0) ; // expecting GT field
		}

		void finalise() {
			// nothing to do
		}

	private:
		Eigen::MatrixXd* m_result ;
		Eigen::MatrixXd* m_nonmissingness ;
		genfile::OrderType m_order_type ;
		int m_result_column ;
		std::size_t m_sample_i ;
	} ;
}

void HaplotypeFrequencyComponent::compute_ld_measures(
	genfile::VariantIdentifyingData const& source_snp,
	genfile::VariantDataReader& source_data_reader,
	genfile::VariantIdentifyingData const& target_snp,
	genfile::VariantDataReader& target_data_reader
) {
	if( source_snp.number_of_alleles() == 2 && target_snp.number_of_alleles() == 2 ) {
		std::vector< int > source_calls ;
		std::vector< uint32_t > source_ploidy ;
		std::vector< int > target_calls ;
		std::vector< uint32_t > target_ploidy ;
		Eigen::MatrixXd dosages( source_data_reader.get_number_of_samples(), 2 ) ;
		Eigen::MatrixXd nonmissingness( source_data_reader.get_number_of_samples(), 2 ) ;

		bool haveCalls = false ;
		try {
			source_data_reader.get( ":genotypes:", CallSetter( &source_calls, &source_ploidy ) ) ;
			target_data_reader.get( ":genotypes:", CallSetter( &target_calls, &target_ploidy ) ) ;
			haveCalls = true ;
		} catch( ... ) {
		}

		{
			DosageSetter source_setter( &dosages, &nonmissingness, 0 ) ;
			DosageSetter target_setter( &dosages, &nonmissingness, 1 ) ;
			source_data_reader.get( ":genotypes:", genfile::to_GP_unphased( source_setter ) ) ;
			target_data_reader.get( ":genotypes:", genfile::to_GP_unphased( target_setter ) ) ;
		}

		// we treat any data point that is missing in one sample as missing in both
		for( int i = 0; i < nonmissingness.rows(); ++i ) {
			if( nonmissingness.row(i).sum() < 2 ) {
				nonmissingness.row(i).setZero() ;
			}
		}

		compute_ld_measures(
			source_snp,
			source_calls,
			source_ploidy,
			target_snp,
			target_calls,
			target_ploidy,
			dosages,
			nonmissingness,
			haveCalls
		) ;
	}
}

void HaplotypeFrequencyComponent::compute_ld_measures(
	genfile::VariantIdentifyingData const& source_snp,
	std::vector< int > const& source_calls,
	std::vector< uint32_t > const& source_ploidy,
	genfile::VariantIdentifyingData const& target_snp,
	std::vector< int > const& target_calls,
	std::vector< uint32_t > const& target_ploidy,
	Eigen::MatrixXd const& dosages,
	Eigen::MatrixXd const& nonmissingness,
	bool const runEM
) {
	if( m_stratification ) {
		bool output = (m_min_r2 == 0.0 ) ;
		for( std::size_t i = 0; i < m_stratification->number_of_strata(); ++i ) {
			try {
				if( runEM ) {
					output = compute_em_ld_measures(
						source_snp,
						source_calls,
						source_ploidy,
						target_snp,
						target_calls,
						target_ploidy,
						m_stratification->stratum_name(i) + ":",
						m_stratification->stratum(i),
						output
					) || output ;
				}
				output = compute_dosage_ld_measures(
					source_snp,
					target_snp,
					dosages,
					nonmissingness,
					m_stratification->stratum_name(i) + ":",
					m_stratification->stratum(i),
					output
				) || output ;
			} catch( genfile::OperationFailedError const& e ) {
				m_ui_context.logger() << "!! HaplotypeFrequencyComponent::compute_ld_measures(): "
					<< "stratum " << m_stratification->stratum_name(i) << ": "
					<< "could not compute LD measures between SNPs "
					<< source_snp << " and " << target_snp << ".\n"
					<< "!! reason: " << e.get_message() << "\n" ;
			}
		}
	} else {
		try {
			bool output = (m_min_r2 == 0.0 ) ;
			if( runEM ) {
				output = compute_em_ld_measures(
					source_snp,
					source_calls, source_ploidy,
					target_snp,
					target_calls, target_ploidy,
					"",
					std::vector< genfile::SampleRange  >( 1, genfile::SampleRange( 0, dosages.rows() ) ),
					output
				) ;
			}
			compute_dosage_ld_measures(
				source_snp,
				target_snp,
				dosages,
				nonmissingness,
				"",
				std::vector< genfile::SampleRange  >( 1, genfile::SampleRange( 0, dosages.rows() ) ),
				output
			) ;
		} catch( genfile::OperationFailedError const& e ) {
			m_ui_context.logger() << "!! HaplotypeFrequencyComponent::compute_ld_measures(): "
				<< "could not compute LD measures between SNPs "
				<< source_snp << " and " << target_snp << ".\n"
				<< "!! reason: " << e.get_message() << "\n" ;
		}
	}
}

bool HaplotypeFrequencyComponent::compute_dosage_ld_measures(
	genfile::VariantIdentifyingData const& source_snp,
	genfile::VariantIdentifyingData const& target_snp,
	Eigen::MatrixXd const& dosages,
	Eigen::MatrixXd const& nonmissingness,
	std::string const& variable_name_stub,
	std::vector< genfile::SampleRange > const& sample_set,
	bool alwaysOutput
) {
	bool includeInOutput = alwaysOutput ;
	Eigen::RowVectorXd means = Eigen::RowVectorXd::Zero(2) ;
	Eigen::RowVectorXd totals = Eigen::RowVectorXd::Zero(2) ;
	for( std::size_t i = 0; i < sample_set.size(); ++i ) {

#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
		std::cerr << sample_set[i].begin() << "-" << sample_set[i].end() << ":\n" ;
		std::cerr << dosages.rows() << "x" << dosages.cols()
			<< ", " << nonmissingness.rows() << "x" << nonmissingness.cols() << ".\n" ;
		std::cerr << dosages.block( 0, 0, std::min( int( dosages.rows() ), 10 ), dosages.cols() ) << ".\n" ;
#endif
		means.array() += (
			dosages.block( sample_set[i].begin(), 0, sample_set[i].end() - sample_set[i].begin(), 2 ).array()
			* nonmissingness.block( sample_set[i].begin(), 0, sample_set[i].end() - sample_set[i].begin(), 2 ).array()
		).colwise().sum() ;

		totals += nonmissingness.block( sample_set[i].begin(), 0, sample_set[i].end() - sample_set[i].begin(), 2 ).colwise().sum() ;
	}

	means.array() /= totals.array() ;

	Eigen::Matrix2d covariance = Eigen::Matrix2d::Zero() ;
	Eigen::Matrix2d nonmissing = Eigen::Matrix2d::Zero() ;
	for( std::size_t i = 0; i < sample_set.size(); ++i ) {
		//Eigen::MatrixBase< Eigen::MatrixXd > block = (
		Eigen::MatrixXd block = (
			(dosages.block( sample_set[i].begin(), 0,  sample_set[i].end() - sample_set[i].begin(), 2 ).rowwise() - means ).array()
				* nonmissingness.block( sample_set[i].begin(), 0,  sample_set[i].end() - sample_set[i].begin(), 2 ).array()
		) ;
		Eigen::Block< Eigen::MatrixXd const > nonmissing_block = nonmissingness.block(
			sample_set[i].begin(), 0,  sample_set[i].end() - sample_set[i].begin(), 2
		) ;

		covariance += block.transpose() * block ;
		nonmissing += nonmissing_block.transpose() * nonmissing_block ;
	}
	// Compute correlation as:
	//
	double r = covariance(0,1) / std::sqrt( covariance(0,0) * covariance(1,1) ) ;

	includeInOutput = includeInOutput || (r*r >= m_min_r2 ) ;

#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
	std::cerr << "means = " << means << ".\n" ;
	std::cerr << "cov =\n" << covariance << ".\n" ;
	std::cerr << "r =\n" << r << ".\n" ;
#endif

	if( includeInOutput ) {
		m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "dosage_r", r  ) ;
		m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "dosage_r2", r*r ) ;
	}
	return includeInOutput ;
}

bool HaplotypeFrequencyComponent::compute_em_ld_measures(
	genfile::VariantIdentifyingData const& source_snp,
	std::vector< int > const& source_calls,
	std::vector< uint32_t > const& source_ploidy,
	genfile::VariantIdentifyingData const& target_snp,
	std::vector< int > const& target_calls,
	std::vector< uint32_t > const& target_ploidy,
	std::string const& variable_name_stub,
	std::vector< genfile::SampleRange > const& sample_set,
	bool alwaysOutput
) {
	// Construct table of genotypes at each SNP.
	Eigen::Matrix3d diploid_table = Eigen::Matrix3d::Zero() ;
	Eigen::Matrix2d haploid_table = Eigen::Matrix2d::Zero() ;

	tabulate_calls(
		sample_set,
		source_calls, source_ploidy,
		target_calls, target_ploidy,
		&diploid_table,
		&haploid_table
	) ;

#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
	std::cerr << "SNP1: " << source_snp << "\n"
		<< "SNP2: " << target_snp << "\n"
		<< "diploids: " << diploid_table << ".\n"
		<< "haploids: " << haploid_table << ".\n" ;
	std::cerr << "alwaysOutput is " << ( alwaysOutput ? "true" : "false" ) << ".\n" ;
#endif

	genfile::VariantEntry::Integer const N = diploid_table.sum() + haploid_table.sum() ;
	bool includeInOutput = alwaysOutput ;
	genfile::VariantEntry const missing = genfile::MissingValue() ;

	if( includeInOutput ) {
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "number_of_genotypes", diploid_table.sum() ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "number_of_haplotypes", haploid_table.sum() ) ;
	}

	if( N == 0 ) {
		if( includeInOutput ) {
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi00", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi01", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi10", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi11", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "D", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "Dprime", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "r", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "r2", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "dosage_r", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "dosage_r2", missing ) ;
		}
	} else {
		try {
			HaplotypeFrequencyLogLikelihood ll( diploid_table, haploid_table + m_prior ) ;
			HaplotypeFrequencyLogLikelihood::Vector pi( 4 ) ;
			pi.tail( 3 ) = ll.get_MLE_by_EM() ;
			pi(0) = 1.0 - pi.tail( 3 ).sum() ;
			double D = pi(0) * pi(3) - pi(1) * pi(2) ;
			double max_D ;
			if( D < 0 ) {
				max_D = std::min( (pi(0) + pi(1)) * (pi(0)+pi(2)), (pi(1)+pi(3))*(pi(2)+pi(3)) ) ;
			}
			else {
				max_D = std::min( (pi(0) + pi(1)) * (pi(1)+pi(3)), (pi(0)+pi(2))*(pi(2)+pi(3)) ) ;
			}
			double const Dprime = D / max_D ;
			double const r = D / std::sqrt( (pi(0)+pi(1)) * (pi(2)+pi(3)) * (pi(0)+pi(2)) * (pi(1)+pi(3))) ;
			double const r2 = r*r ;
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
			std::cerr << "r = \n" << std::setprecision(3) << r << ".\n" ;
			std::cerr << "r^2 = " << r2 << ".\n" ;
			std::cerr << "D =\n" << D << ".\n" ;
			std::cerr << "Dprime =\n" << Dprime << ".\n" ;
			std::cerr << "pi = " << pi.transpose() << "\n" ;
#endif

			includeInOutput = includeInOutput || (r2 >= m_min_r2 ) ;
			if( includeInOutput ) {
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi00", pi(0) ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi01", pi(1) ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi10", pi(2) ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi11", pi(3) ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "D", D ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "Dprime", Dprime ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "r", r ) ;
				m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "r2", r2 ) ;
			}
#if DEBUG_HAPLOTYPE_FREQUENCY_COMPONENT
		std::cerr << "Output complete.\n" ;
#endif
		}
		catch( genfile::OperationFailedError const& ) {
			m_ui_context.logger() << "!! Could not compute haplotype frequencies for " << source_snp << ", " << target_snp << ".\n"
				<< "!! table is:\n" << diploid_table << ".\n" ;

			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi00", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi01", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi10", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "pi11", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "D", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "Dprime", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "r", missing ) ;
			m_sink->store_per_variant_pair_data( source_snp, target_snp, variable_name_stub + "r2", missing ) ;
		}

	}
	return includeInOutput ;
}

void HaplotypeFrequencyComponent::end_processing_snps() {
	if( m_sink.get() ) {
		m_sink->finalise() ;
	}
}

void HaplotypeFrequencyComponent::send_results_to( haplotype_frequency_component::FlatTableDBOutputter::UniquePtr sink ) {
	m_sink = sink ;
}


#include <boost/intrusive/Fenwick_tree/fenwick_tree_algorithms.hpp>
#include <boost/intrusive/Fenwick_tree/fenwick_tree_hook.hpp>
#include <boost/intrusive/Fenwick_tree/fenwick_tree_iterator.hpp>

#include<boost/intrusive/any_hook.hpp>
#include <boost/intrusive/detail/get_value_traits.hpp>
#include "boost/intrusive/options.hpp"
#include <boost/intrusive/detail/is_stateful_value_traits.hpp>
#include <boost/intrusive/detail/default_header_holder.hpp>
#include "boost/intrusive/detail/size_holder.hpp"
#include<iostream>
#include <boost/intrusive/detail/config_begin.hpp>
#include <boost/intrusive/intrusive_fwd.hpp>
#include <boost/intrusive/detail/assert.hpp>
#include <boost/intrusive/pointer_traits.hpp>
#include <boost/intrusive/detail/mpl.hpp>
#include <boost/intrusive/link_mode.hpp>
#include <boost/intrusive/detail/reverse_iterator.hpp>
#include <boost/intrusive/detail/uncast.hpp>
#include <boost/intrusive/detail/array_initializer.hpp>
#include <boost/intrusive/detail/exception_disposer.hpp>
#include <boost/intrusive/detail/equal_to_value.hpp>
#include <boost/intrusive/detail/key_nodeptr_comp.hpp>
#include <boost/intrusive/detail/simple_disposers.hpp>
#include <boost/intrusive/detail/algorithm.hpp>

#include <boost/move/utility_core.hpp>
#include <boost/static_assert.hpp>

#include <boost/intrusive/detail/minimal_less_equal_header.hpp>//std::less
#include <cstddef>   //std::size_t, etc.

#include <queue>

namespace boost {
    namespace intrusive {

struct default_fenwick_tree_hook_applier
{
    template <class T>
    struct apply
    {
        typedef typename T::default_fenwick_tree_hook type;
    };
};

template<>
struct is_default_hook_tag<default_fenwick_tree_hook_applier>
{  static const bool value = true;  };

struct fenwick_tree_defaults
{
   typedef default_fenwick_tree_hook_applier proto_value_traits;
   static const bool constant_time_size = true;
   typedef std::size_t size_type;
   typedef void header_holder_type;
};
/*!
This class contains all the basic methods supported by fenwick tree.
*/
template<typename ValueTraits, class SizeType, bool ConstantTimeSize, typename HeaderHolder>
class fenwick_tree_impl
{
    public:
    typedef ValueTraits                                               value_traits;
    typedef typename value_traits::node_traits node_traits;
    typedef typename node_traits::node node;
    typedef typename node::node_ptr node_ptr;
    typedef typename value_traits::pointer pointer;
    typedef typename pointer_traits<pointer>::element_type            value_type;
    typedef typename pointer_traits<pointer>::reference               reference;
    typedef fenwick_tree_algorithms<node_traits> algo;
    typedef typename node_traits::const_node_ptr                      const_node_ptr;
    typedef SizeType                                                  size_type;
    typedef fenwick_tree_iterator<value_traits, false>                        iterator;
    typedef fenwick_tree_iterator<value_traits, true>                         const_iterator;
    typedef typename value_type::data_type data_type;
    typedef typename detail::get_header_holder_type
      < value_traits, HeaderHolder >::type                           header_holder_type;
   private:
   ///@cond
   static const bool constant_time_size = ConstantTimeSize;
   static const bool stateful_value_traits = detail::is_stateful_value_traits<value_traits>::value;
   static const bool has_container_from_iterator =
        detail::is_same< header_holder_type, detail::default_header_holder< node_traits > >::value;

   ///@endcond
   private:
    typedef detail::size_holder<constant_time_size, size_type>          size_traits;

   node_ptr get_root_node()
   { return data_.root_plus_size_.m_header.get_node(); }

   const_node_ptr get_root_node() const
   { return data_.root_plus_size_.m_header.get_node(); }

   struct root_plus_size : public size_traits
   {
      header_holder_type m_header;
   };

   struct data_t : public ValueTraits
   {
      typedef typename fenwick_tree_impl::value_traits value_traits;
      root_plus_size root_plus_size_;
   } data_;

   size_traits &priv_size_traits()
   {  return data_.root_plus_size_;  }

   const size_traits &priv_size_traits() const
   {  return data_.root_plus_size_;  }

   const value_traits &priv_value_traits() const
   {
     return data_;
   }

   value_traits &priv_value_traits()
   {
      return data_;
   }

   typedef typename boost::intrusive::value_traits_pointers
      <ValueTraits>::const_value_traits_ptr const_value_traits_ptr;

   const_value_traits_ptr priv_value_traits_ptr() const
   {  return pointer_traits<const_value_traits_ptr>::pointer_to(this->priv_value_traits());  }

    private:

    value_type *ptr;
    value_type *input;

    int start,end;
    int total_nodes=0,max_index=0,level_one_nodes=0;
    int leaf_nodes=0;

    node_ptr root;

   public:
    /*!
    This is a initialisation function where all the variables required for supporting fenwick tree are created and initialised with apporopriate values.
    \param input input array
    \param start starting index of input array
    \param end last index of input array
    \return Nothing

     Complexity  O(N)
    */
    fenwick_tree_impl(value_type input[],int start,int end)
    {
        this->input=input;
        this->start=start;
        this->end=end;
        total_nodes=end-start+2;
        leaf_nodes=(total_nodes-1)/2+(total_nodes-1)%2;
        max_index=total_nodes-1;
        ptr=(value_type*)malloc((total_nodes-leaf_nodes)*sizeof(value_type));
        root=value_traits::to_node_ptr(ptr[0]);
        int nearest_pow=1;
        while(nearest_pow<=max_index)
        {
            level_one_nodes++;
            nearest_pow*=2;
        }
        root->children=(node_ptr*)malloc((level_one_nodes)*sizeof(node_ptr));
        int nt=0;
        std::queue<int> node_index;
        node_index.push(0);
        std::map<int,int> index_new,new_index;
        while(!node_index.empty())
        {
            int index=node_index.front();
            node_ptr temp_node;
            if(index%2==1)
            {
                temp_node=value_traits::to_node_ptr(input[index-1]);
            }
            else
            {
                temp_node=value_traits::to_node_ptr(ptr[nt]);
                index_new.insert(std::make_pair(nt,index));
                new_index.insert(std::make_pair(index,nt));
                nt++;
            }
            int temp=1,child_cnt=0;
            while((index&temp)!=temp && (index|temp)<=max_index)
            {
                child_cnt++;
                node_index.push(index|temp);
                temp*=2;
            }
            temp_node->children=(node_ptr*)malloc(child_cnt*sizeof(node_ptr));
            temp_node->child_cnt=child_cnt;
            node_index.pop();
        }
        for(int i=0;i<total_nodes-leaf_nodes;i++)
        {
            node_ptr node=value_traits::to_node_ptr(ptr[i]);
            int index=index_new[i];
            int temp=1,child_par=0;
            while((index&temp)!=temp && (index|temp)<=max_index)
            {
                int a=index|temp;
                if(temp==1)
                {
                    node->children[child_par]=value_traits::to_node_ptr(input[(index|temp)-1]);
                }
                else
                {
                    node->children[child_par]=value_traits::to_node_ptr(ptr[new_index[index|temp]]);
                }
                child_par++;
                temp*=2;
            }
        }
    }
    private:
        int cnt_run=1;
    public:
    /*!
    This function builds the fenwick tree based on the given inputs
    \param func merging function
    \return Nothing

    Complexity :   O(Nlog(N))    */
    void build(auto func)
    {
        for(int i=0;i<total_nodes-leaf_nodes;i++)
        {
            ptr[i].value.a=0;
        }
        build_computation(func);
    }
    private:
    void build_computation(auto func)
    {
        for(int i=0;i<=end;i++)
        {
            update(func,i,input[i]);
        }
    }
    public:
    /*!
    This function updates fenwick tree according to the given inputs
    \param func merging function
    \param index updated index
    \param val updated_value
    \return Nothing

    Complexity :   O(log(N))
    */
    void update(auto func,int index,value_type val)
    {
        std::stack<int> bit_pos;
        int temp=1,position=0;
        while(temp<=index+1)
        {
            int sb=temp&(index+1);
            if(sb==temp)
            {
                bit_pos.push(position);
            }
            position++;
            temp*=2;
        }
        update_computation(root,func,bit_pos,val);
    }
    private:
    void update_computation(node_ptr curr_node,auto func,std::stack<int> &bit_pos,value_type val)
    {
        while(bit_pos.size()!=0)
        {
            int child=bit_pos.top();
            bit_pos.pop();
            int curr_cnt=curr_node->child_cnt;
            int size=bit_pos.size();
            if(bit_pos.size()!=0)
            {
                for(int i=child+1;i<curr_cnt;i++)
                {
                    if(i!=0)
                    {
                        pointer p=value_traits::to_value_ptr(curr_node->children[i]);
                        p->value=func(p->value,val.value);
                    }
                }
            }
            else
            {
                for(int i=child;i<curr_cnt;i++)
                {
                    if(i!=0)
                    {
                        pointer p=value_traits::to_value_ptr(curr_node->children[i]);
                        p->value=func(p->value,val.value);
                    }
                }
            }
            curr_node=curr_node->children[child];
        }
    }
    data_type curr_value;
    public:
    /*!
    This function queries the fenwick tree and returns the answer corresponding to the given inputs.
    \param func merging function
    \param index index till which fenwick tree needs to be queried.
    \return the query value over the range given in input.

    Complexity :   O(log(N))
    */
    data_type query(auto func,int index)
    {
        std::stack<int> bit_pos;
        int temp=1,cnt=0;
        while(temp<=index+1)
        {
            int a=temp&(index+1);
            if(a==temp)
            {
                bit_pos.push(cnt);
            }
            cnt++;
            temp*=2;
        }
        query_computation(root,func,bit_pos);
        return curr_value;
    }
    private:
    void query_computation(node_ptr curr_node,auto func,std::stack<int> &bit_pos)
    {
        int flag=1;
        while(!bit_pos.empty())
        {
            int child=bit_pos.top();
            bit_pos.pop();
            pointer p=value_traits::to_value_ptr(curr_node->children[child]);
            if(flag)
            {
                curr_value=p->value;
                flag=0;
            }
            else
            {
                curr_value=func(curr_value,p->value);
            }
            curr_node=curr_node->children[child];
        }
    }
    public:
    /*!
This returns an iterator to the root node or source node of fenwick tree.
*/
    iterator get_root()
    {
        return iterator(root,this->priv_value_traits_ptr());
    }
};

#if defined(BOOST_INTRUSIVE_DOXYGEN_INVOKED) || defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
template<class T, class ...Options>
#else
template<class T, class O1 = void, class O2 = void, class O3 = void, class O4 = void>
#endif
struct make_fenwick_tree
{
   public:
   typedef typename pack_options
      < fenwick_tree_defaults,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
         O1, O2, O3, O4
         #else
         Options...
         #endif
         >::type packed_options;

   typedef typename detail::get_value_traits
      <T, typename packed_options::proto_value_traits>::type value_traits;
   /*!

This is the main class which contains all the methods supported by fenwick tree.

This class is derived into "fenwick_tree" class by giving appropriate inputs.

   */
   typedef fenwick_tree_impl
      <value_traits,
        typename packed_options::size_type,
         packed_options::constant_time_size,
         typename packed_options::header_holder_type
   > implementation_defined;
   typedef implementation_defined type;
};

#ifndef BOOST_INTRUSIVE_DOXYGEN_INVOKED

#if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
template<class T, class O1, class O2, class O3, class O4>
#else
template<class T, class ...Options>
#endif
class fenwick_tree
   : public make_fenwick_tree<T,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4
      #else
      Options...
      #endif
   >::type
{
    public:
       typedef typename make_fenwick_tree
      <T,
      #if !defined(BOOST_INTRUSIVE_VARIADIC_TEMPLATES)
      O1, O2, O3, O4
      #else
      Options...
      #endif
      >::type      Base;

 public:
   typedef typename Base::value_traits          value_traits;

    public:
    /*!

 This is the first and main function used while working with any data structure.

This calls the "fenwick_tree_impl" function which does initialisation and declaration of variables.

    */
    fenwick_tree(T input[],int start,int end)
        : Base(input,start,end)
    {

    };
};

}
}
#endif


#include "lipton-tarjan.h"
#include "colors.h"
#include "strutil.h"
#include <iostream>
#include <vector>
#include <algorithm>
#include <utility>
#include <csignal>
#include <boost/lexical_cast.hpp>
#include <boost/graph/graph_concepts.hpp>
#include <boost/graph/properties.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/property_map/property_map.hpp>
#include <boost/graph/planar_canonical_ordering.hpp>
#include <boost/graph/is_straight_line_drawing.hpp>
#include <boost/graph/boyer_myrvold_planar_test.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/make_biconnected_planar.hpp>
#include <boost/graph/make_maximal_planar.hpp>
#include <boost/graph/connected_components.hpp>
#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/breadth_first_search.hpp>
#include <boost/pending/indirect_cmp.hpp>
#include <boost/range/irange.hpp>
#include <boost/graph/copy.hpp>
using namespace std;
using namespace boost;
#define STLALL(x) (x).begin(), (x).end()

#define HEADER_COL GREEN

int levi_civita(uint i, uint j, uint k)
{
        if( i == j || j == k || k == i ) return 0;
        if( i == 1 && j == 2 && k == 3 ) return 1;
        if( i == 2 && j == 3 && k == 1 ) return 1;
        if( i == 3 && j == 1 && k == 2 ) return 1;
        return -1;
}

bool on_cycle(VertDesc v, vector<VertDesc> const& cycle, Graph const& g)
{
        return find(STLALL(cycle), v) != cycle.end();
}

bool on_cycle(EdgeDesc e, vector<VertDesc> const& cycle, Graph const& g)
{
        auto src = source(e, g);
        auto tar = target(e, g);
        return on_cycle(src, cycle, g) && on_cycle(tar, cycle, g);
}

enum InsideOut {INSIDE, OUTSIDE, ON};

InsideOut edge_inside_cycle(EdgeDesc e, VertDesc common_vert, vector<VertDesc> const& cycle, Graph const& g, Embedding const& em)
{
        //cout << "cycle: ";
        for( uint i = 0; i < cycle.size(); ++i ) cout << cycle[i] << ' ';
        //cout << '\n';
        auto src = source(e, g);
        auto tar = target(e, g);
        if( on_cycle(e, cycle, g) ) return ON;
        //cout << "      testing if edge " << src << ", " << tar << " is inside the cycle: ";
        //cout << "      common_vert:    " << common_vert   << '\n';
        auto it     = find(STLALL(cycle), common_vert);
        if( it == cycle.end() ){ cout << "      not here at all!\n"; assert(0); }
        assert(*it == common_vert);
        auto before = it   == cycle.begin() ?  cycle.end  ()-1   : it-1;
        auto after  = it+1 == cycle.end  () ?  cycle.begin()     : it+1;
        auto other  = (source(e, g) == common_vert) ?  target(e, g)        : source(e, g);

        //cout << '\n';
        //cout << "      it:     " << *it         << '\n';
        //cout << "      v:      " << common_vert << '\n';
        //cout << "      before: " << *before     << '\n';
        //cout << "      after:  " << *after      << '\n';
        //cout << "      other:  " << other       << '\n';

        vector<uint> perm;
        set<VertDesc> seenbefore;
        for( auto& tar_it : em[*it] ){ // why does this contain duplicates?
                auto src = source(tar_it, g);
                auto tar = target(tar_it, g);
                if( src != common_vert ) swap(src, tar);
                assert(src == common_vert);
                if( seenbefore.find(tar) != seenbefore.end() ) continue;
                seenbefore.insert(tar);

                if(      tar == other   ) perm.push_back(1);
                else if( tar == *before ) perm.push_back(2);
                else if( tar == *after  ) perm.push_back(3);
        }
        assert(perm.size() == 3);
        if( levi_civita(perm[0], perm[1], perm[2]) == 1 ){ return INSIDE;
        } else { return OUTSIDE; }
}

struct BFSVert
{
        BFSVert() : parent(Graph::null_vertex()), level(0), descendant_cost(0) {}

        VertDesc parent;
        int      level;
        uint     descendant_cost;
};

struct BFSVisitorData
{
        map<VertDesc, set<VertDesc>> children;
        map<VertDesc, BFSVert>       verts;
        int                          num_levels;
        Graph*                       g;
        VertDesc                     root;

        BFSVisitorData(Graph* g) : g(g), num_levels(0), root(Graph::null_vertex()) {}

        void reset(Graph* g)
        {
                children.clear();
                verts   .clear();
                num_levels = 0;
                this->g = g;
                root = Graph::null_vertex();
        }

        bool is_tree_edge(EdgeDesc e) const
        {
                //cout << "testing is tree edge " << to_string(e, *g) << '\n';
                auto src = source(e, *g);
                auto tar = target(e, *g);
                auto src_it = verts.find(src);
                auto tar_it = verts.find(tar);
                //cout << "src: " << src << '\n';
                //cout << "tar: " << tar << '\n';
                assert(src_it != verts.end());
                assert(tar_it != verts.end());
                return src_it->second.parent == tar || tar_it->second.parent == src;
        }

        uint edge_cost(EdgeDesc e, vector<VertDesc> const& cycle, Graph const& g) const
        {
                assert(is_tree_edge(e));

                auto v = source(e, g);
                auto w = target(e, g);
                auto v_it = verts.find(v); assert(v_it != verts.end());
                auto w_it = verts.find(w); assert(w_it != verts.end());
                if( !on_cycle(v, cycle, g) ) swap(v, w);

                assert( on_cycle(v, cycle, g));
                assert(!on_cycle(w, cycle, g));

                uint total = num_vertices(g);

                assert(w_it->second.parent == v || v_it->second.parent == w);
                return w_it->second.parent == v ? w_it->second.descendant_cost : total - v_it->second.descendant_cost;
        }

        void print_costs  () const {for( auto& v : verts ) cout << "descendant cost of vertex " << v.first << " is " << v.second.descendant_cost << '\n';}
        void print_parents() const {for( auto& v : verts ) cout << "parent of " << v.first << " is " << v.second.parent << '\n';}
};

struct BFSVisitor : public default_bfs_visitor
{
        BFSVisitorData& data;

        BFSVisitor(BFSVisitorData& data) : data(data) {}

        template<typename Edge, typename Graph> void tree_edge(Edge e, Graph const& g)
        {
                auto parent = source(e, g);
                auto child  = target(e, g);
                cout << "  tree edge " << parent << ", " << child << '\n';
                data.verts[child].parent = parent;
                data.verts[child].level  = data.verts[parent].level + 1;
                data.num_levels = max(data.num_levels, data.verts[child].level + 1);
                if( Graph::null_vertex() != parent ) data.children[parent].insert(child);

                VertDesc v = child;
                data.verts[v].descendant_cost = 1;
                //cout << "     vertex/descendant cost: ";
                //cout << v << '/'  << data.verts[v].descendant_cost << "   ";
                while( data.verts[v].level ){
                        v =  data.verts[v].parent;
                        ++data.verts[v].descendant_cost;
                        //cout << v << '/'  << data.verts[v].descendant_cost << "   ";
                }
                //cout << '\n';
        }
};

uint theorem4(uint partition, Graph const& g)
{
        /*
        Assume G is connected.
        Partition the vertices into levels according to their distance from some vertex v.
        L[l] = # of vertices on level l
        If r is the maximum distance of any vertex from v, define additional levels -1 and r+1 containing no vertices
        l1 = the level such that the sum of costs in levels 0 thru l1-1 < 1/2, but the sum of costs in levels 0 thru l1 is >= 1/2
        (If no such l1 exists, the total cost of all vertices < 1/2, and B = C = {} and return true)
        k = # of vertices on levels 0 thru l1.
        Find a level l0 such that l0 <= l1 and |L[l0]| + 2(l1-l0) <= 2sqrt(k)
        Find a level l2 such that l1+1 <= l2 and |L[l2] + 2(l2-l1-1) <= 2sqrt(n-k)
        If 2 such levels exist, then by Lemma 3 the vertices of G can be partitioned into three sets A, B, C such that no edge joins a vertex in A with a vertex in B,
        neither A or C has cost > 2/3, and C contains no more than 2(sqrt(k) + sqrt(n-k)) vertices.
        But 2(sqrt(k) + sqrt(n-k) <= 2(sqrt(n/2) + sqrt(n/2)) = 2sqrt(2)sqrt(n)
        Thus the theorem holds if suitable levels l0 and l2 exist
                Suppose a suitable level l0 does not exist.  Then, for i <= l1, L[i] >= 2sqrt(k) - 2(l1-i)
                Since L[0] = 1, this means 1 >= 2sqrt(k) - 2l1 and l1 + 1/2 >= sqrt(k).  Thus l1 = floor(l1 + 1/2) >
                Contradiction

        Now suppose G is not connected
        Let G1, G2, ... , Gk be the connected components of G, with vertex sets V1, V2, ... , Vk respectively.
        If no connected component has total vertex cost > 1/3, let i be the minimum index such that the total cost of V1 U V2 U ... U Vi > 1/3
        A = V1 U V2 U ... U Vi
        B = Vi+1 U Vi+2 U ... U Vk
        C = {}
        Since i is minimum and the cost of Vi <= 1/3, the cost of A <= 2/3. return true;
        If some connected component (say Gi) has total vertex cost between 1/3 and 2/3,
        A = Vi
        B = V1 U ... U Vi-1 U Vi+1 U ... U Vk
        C = {}
        return true

        Finally, if some connected component (say Gi) has total vertex cost exceeding 2/3,
        apply the above argument to Gi
        Let A*, B*, C* be the resulting partition.
        A = set among A* and B* with greater cost
        C = C*
        B = remanining vertices of G
        Then A and B have cost <= 2/3, g
        return true;

        In all cases the separator C is either empty or contained in only one connected component of G
        */
        return partition;
}

struct ScanVisitor
{
        map<VertDesc, bool>*          table;
        Graph*                        g;
        VertDesc                      x;
        int                           l0;
        set<pair<VertDesc, VertDesc>> edges_to_add, edges_to_delete;

        ScanVisitor(map<VertDesc, bool>* table, Graph* g, VertDesc x, int l0) : table(table), g(g), x(x), l0(l0) {}

        void foundedge(VertDesc V, EdgeDesc e)
        {
                auto v = source(e, *g);
                auto w = target(e, *g);
                if( V != v ) swap(v, w);
                assert(V == v);
                cout << "foundedge " << v << ", " << w;
                if ( !(*table)[w] ){
                        (*table)[w] = true;
                        assert(x != w);
                        cout << "   !!!!!!!going to add " << x << ", " << w;
                        edges_to_add.insert(make_pair(x, w));
                }
                cout << "     going to delete " << v << ", " << w;
                edges_to_delete.insert(make_pair(v, w));
                cout << '\n';
        }

        void finish()
        {
                cout << "finishing\n";
                cout << "edges to add size: " << edges_to_add.size() << '\n';
                cout << "adding: ";
                for( auto& p : edges_to_add    ){
                        assert(p.first != p.second);
                        cout << '(' << p.first << ", " << p.second << ")   ";
                        add_edge(p.first, p.second, *g);
                }
                cout << '\n';
                cout << "edges to remove size: " << edges_to_delete.size() << '\n';
                cout << "removing: ";
                for( auto& p : edges_to_delete ){
                        cout << '(' << p.first << ", " << p.second << ")   ";
                        remove_edge(p.first, p.second, *g);
                }
                cout << '\n';
        }

        void scan_nonsubtree_edges(VertDesc v, Graph const& g, Embedding const& em, BFSVisitorData const& bfs)
        {
                auto v_it = bfs.verts.find(v);
                assert(v_it != bfs.verts.end());
                if( v_it->second.level > l0 ) return;
                for( auto e : em[v] ){
                        auto src = source(e, g);
                        auto tar = target(e, g);
                        if( src == tar ) continue; // ?????

                        if( !bfs.is_tree_edge(e) ){
                                foundedge(v, e);
                                continue;
                        }
                        if( src != v ) swap(src, tar);
                        assert(src == v);
                        auto tar_it = bfs.verts.find(tar);
                        if( tar_it->second.level > l0 ) foundedge(v, e);
                }
                //cout << "looking for children of " << v << '\n';
                auto vvv   = bfs.children.find(v);
                if( vvv == bfs.children.end() ) return; // no children
                //cout << "from " << v << " looking for children\n";
                for( auto& c : vvv->second ){
                        //cout << "child: " << c << '\n';
                        scan_nonsubtree_edges(c, g, em, bfs);
                }
        }

};

void reset_vertex_indices(Graph& g)
{
        VertIter vi, vend;
        uint i = 0;
        for( tie(vi, vend) = vertices(g); vi != vend; ++vi, ++i ) put(vertex_index, g, *vi, i);
}

struct Em
{
        EmbeddingStorage* storage;
        Embedding*        em;
        Graph*            g;

        Em(Graph* g) : g(g), storage(new EmbeddingStorage(num_vertices(*g))), em(new Embedding(storage->begin()))
        {
                testplanar();
        }

        bool testplanar() {return boyer_myrvold_planarity_test(boyer_myrvold_params::graph = *g, boyer_myrvold_params::embedding = *em);}

        void print()
        {
                cout << CYAN << "\n************** Embedding ************\n" << RESET;
                VertIter vi, vend;
                for( tie(vi, vend) = vertices(*g); vi != vend; ++vi ){
                        cout << "vert " << *vi << ": ";
                        Embedding& embedding = *em;
                        for( auto ei = embedding[*vi].begin(); ei != embedding[*vi].end(); ++ei ){
                                auto src = source(*ei, *g);
                                auto tar = target(*ei, *g);
                                if( tar == *vi ) swap(src, tar);
                                cout << tar << ' ';
                        }
                        cout << "\n";
                }
                cout << CYAN << "*************************************\n" << RESET;
        }

        ~Em()
        {
                delete storage;
                delete em;
        }
};

EdgeIndex reset_edge_index(Graph const& g)
{
        EdgeIndex edgedesc_to_uint;
        EdgesSizeType num_edges = 0;
        EdgeIter ei, ei_end;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ) edgedesc_to_uint[*ei] = num_edges++;
        return edgedesc_to_uint;
}

void makemaxplanar(Graph& g)
{
        auto index = reset_edge_index(g);
        Em em(&g);
        em.testplanar();
        make_biconnected_planar(g, *em.em, index);

        reset_edge_index(g);
        em.testplanar();

        make_maximal_planar(g, *em.em);

        reset_edge_index(g);
        assert(em.testplanar());
}

vector<VertDesc> ancestors(VertDesc v, BFSVisitorData const& vis)
{
        //cout << "first v: " << v << '\n';
        //cout << "root: " << vis.root << '\n';
        vector<VertDesc> ans = {v};
        while( v != vis.root ){
                auto v_it = vis.verts.find(v);
                assert(v_it != vis.verts.end());
                v = v_it->second.parent;
                ans.push_back(v);
                //cout << "pushing back v: " << v << '\n';
        }
        return ans;
}

VertDesc common_ancestor(vector<VertDesc> const& ancestors_v, vector<VertDesc> const& ancestors_w, BFSVisitorData const& vis)
{
        uint i, j;
        for( i = 0; i < ancestors_v.size(); ++i ) for( j = 0; j < ancestors_w.size(); ++j ) if( ancestors_v[i] == ancestors_w[j] ) return ancestors_v[i];
        assert(0);
        return ancestors_v[i];
}

vector<VertDesc> get_cycle(VertDesc v, VertDesc w, VertDesc ancestor, BFSVisitorData const& vis_data)
{
        vector<VertDesc> cycle, tmp;
        VertDesc cur;
        cur = v; while( cur != ancestor ){ cycle.push_back(cur); auto cur_it = vis_data.verts.find(cur); cur = cur_it->second.parent; } cycle.push_back(ancestor);
        cur = w; while( cur != ancestor ){ tmp  .push_back(cur); auto cur_it = vis_data.verts.find(cur); cur = cur_it->second.parent; }
        reverse(STLALL(tmp));
        cycle.insert(cycle.end(), STLALL(tmp));
        return cycle;
}

vector<VertDesc> get_cycle(VertDesc v, VertDesc w, BFSVisitorData const& vis_data)
{
        auto parents_v   = ancestors(v, vis_data);
        auto parents_w   = ancestors(w, vis_data);
        auto ancestor    = common_ancestor(parents_v, parents_w, vis_data);
        cout << "common ancestor: " << ancestor << '\n';
        return get_cycle(v, w, ancestor, vis_data);
}

void kill_vertex(VertDesc v, Graph& g)
{
        cout << "killing vertex " << v << '\n';
        auto i = vert2uint[v];
        uint2vert.erase(i);
        vert2uint.erase(v);
        clear_vertex(v, g);
        remove_vertex(v, g);
}

EdgeDesc arbitrary_nontree_edge(Graph const& g, BFSVisitorData const& vis_data)
{
        EdgeIter ei, ei_end;
        for( tie(ei, ei_end) = edges(g); ei != ei_end; ++ei ){
                auto src = source(*ei, g);
                auto tar = target(*ei, g);
                assert(edge(src, tar, g).second); // exists
                assert(src != tar);
                if( !vis_data.is_tree_edge(*ei) ) break;
        }
        assert(ei != ei_end);
        assert(!vis_data.is_tree_edge(*ei));
        EdgeDesc chosen_edge = *ei;
        cout << "arbitrarily choosing nontree edge: " << to_string(chosen_edge, g) << '\n';
        return chosen_edge;
}

set<VertDesc> get_neighbors(VertDesc v, Graph const& g)
{
        set<VertDesc> neighbors;
        OutEdgeIter e_cur, e_end;
        for( tie(e_cur, e_end) = out_edges(v, g); e_cur != e_end; ++e_cur ){ auto ne = target(*e_cur, g); neighbors.insert(ne); cout << "      vertex " << v << " has neighbor " << ne << '\n'; }
        return neighbors;
}

set<VertDesc> get_intersection(set<VertDesc> const& a, set<VertDesc> const& b)
{
        set<VertDesc> c;
        set_intersection(STLALL(a), STLALL(b), inserter(c, c.begin()));
        for( auto& i : c ) cout << "      set intersection: " << i << '\n';
        assert(c.size() == 2);
        return c;
}

struct CycleCost
{
        uint inside {}, outside {};
};

bool cost_swapped = false;

CycleCost compute_cycle_cost(vector<VertDesc> const& cycle, Graph const& g, BFSVisitorData const& vis_data, Em const& em)
{
        CycleCost cc;
        for( auto& v : cycle ){
                //cout << "   scanning cycle vert " << v << '\n';
                for( auto e = out_edges(v, g); e.first != e.second; ++e.first ) if( vis_data.is_tree_edge(*e.first) && !on_cycle(*e.first, cycle, g) ){
                        uint cost = vis_data.edge_cost(*e.first, cycle, g);
                        //cout << "      scanning incident tree edge " << to_string(*e.first, g) << "   cost: " << cost << '\n';
                        auto insideout = edge_inside_cycle(*e.first, v, cycle, g, *em.em);
                        assert(insideout != ON);
                        bool is_inside = (insideout == INSIDE);
                        (is_inside ? cc.inside : cc.outside) += cost;
                }
        }
        return cc;
}

struct NotPlanar
{
};

Partition lipton_tarjan(Graph& g, Graph& g_orig)
{
        cout << HEADER_COL << "---------------------------- 1 - Check Planarity  ------------\n" << RESET;
        Em em1(&g);
        if( !em1.testplanar() ) throw NotPlanar();
        cout << "planar ok\n";
        print_graph(g);

        cout << HEADER_COL << "---------------------------- 2 - Connected Components --------\n" << RESET;
        VertDescMap idx;
        associative_property_map<VertDescMap> vertid_to_component(idx);
        VertIter vit, vjt;
        tie(vit, vjt) = vertices(g);
        for( uint i = 0; vit != vjt; ++vit, ++i ) put(vertid_to_component, *vit, i);
        uint components = connected_components(g, vertid_to_component);

        cout << "# of components: " << components << '\n';
        vector<uint> verts_per_comp(components, 0);
        for( tie(vit, vjt) = vertices(g); vit != vjt; ++vit ) ++verts_per_comp[vertid_to_component[*vit]];
        uint biggest_component = 0;
        uint biggest_size      = 0;
        bool too_big           = false;
        for( uint i = 0; i < components; ++i ){
                if( 3*verts_per_comp[i] > 2*num_vertices(g) ){
                        cout << "too big\n";
                        too_big = true;
                }
                if( verts_per_comp[i] > biggest_size ){
                        biggest_size = verts_per_comp[i];
                        biggest_component = i;
                }
        }

        if( !too_big ){
                theorem4(0, g);
                return {};
        }
        cout << "biggest component: " << biggest_component << '\n';

        cout << HEADER_COL << "---------------------------- 3 - BFS and Levels ------------\n" << RESET;
        BFSVisitorData vis_data(&g);
        auto root = *vertices(g).first;
        vis_data.root = root;
        breadth_first_search(g, root, visitor(BFSVisitor(vis_data)));

        vector<uint> L(vis_data.num_levels + 1, 0);
        for( auto& d : vis_data.verts ) ++L[d.second.level];

        for( tie(vit, vjt) = vertices(g); vit != vjt; ++vit ) cout << "level/cost of vert " << *vit << ": " << vis_data.verts[*vit].level << '\n';
        for( uint i = 0; i < L.size(); ++i ) cout << "L[" << i << "]: " << L[i] << '\n';

        cout << HEADER_COL << "---------------------------- 4 - l1 and k  ------------\n" << RESET;
        uint k = L[0];
        int l[3];
        l[1] = 0;
        while( k <= num_vertices(g)/2 ) k += L[++l[1]];
        cout << "k:  " << k    << "      # of verts in levels 0 thru l1\n";
        cout << "l1: " << l[1] << "      total cost of levels 0 thru l1 barely exceeds 1/2\n";

        cout << HEADER_COL << "---------------------------- 5 - Find More Levels -------\n" << RESET;
        float sq  = 2 * sqrt(k);
        float snk = 2 * sqrt(num_vertices(g) - k);
        cout << "sq:    " << sq << '\n';
        cout << "snk:   " << snk << '\n';

        l[0] = l[1];     for( ;; ){ float val = L.at(l[0]) + 2*(l[1] - l[0]);     if( val <= sq  ) break; --l[0]; } cout << "l0: " << l[0] << "     highest level <= l1\n";
        l[2] = l[1] + 1; for( ;; ){ float val = L.at(l[2]) + 2*(l[2] - l[1] - 1); if( val <= snk ) break; ++l[2]; } cout << "l2: " << l[2] << "     lowest  level >= l1 + 1\n";

        cout << HEADER_COL << "---------------------------- 6 - Shrinktree -------------\n" << RESET;
        cout << "n: " << num_vertices(g) << '\n';

        vector<VertDesc> replaceverts;
        tie(vit, vjt) = vertices(g);
        for( auto next = vit; vit != vjt; vit = next ){
                ++next;
                if( vis_data.verts[*vit].level >= l[2] ){
                        cout << "deleting vertex " << *vit << " of level l2 " << vis_data.verts[*vit].level << " >= " << l[2] << '\n';
                        kill_vertex(*vit, g);
                }
                if( vis_data.verts[*vit].level <= l[0] ){
                        cout << "going to replace vertex " << *vit << " of level l0 " << vis_data.verts[*vit].level << " <= " << l[0] << '\n';
                        replaceverts.push_back(*vit);
                }
        }

        auto x = add_vertex(g); uint2vert[vert2uint[x] = 999999] = x;
        map<VertDesc, bool> t;
        for( tie(vit, vjt) = vertices(g); vit != vjt; ++vit ){
                t[*vit] = vis_data.verts[*vit].level <= l[0];
                cout << "vertex " << *vit << " at level " << vis_data.verts[*vit].level << " is " << (t[*vit] ? "TRUE" : "FALSE") << '\n';
        }

        reset_vertex_indices(g);
        reset_edge_index(g);
        Em em(&g);
        assert(em.testplanar());

        ScanVisitor svis(&t, &g, x, l[0]);
        svis.scan_nonsubtree_edges(*vertices(g).first, g, *em.em, vis_data);
        svis.finish();

        auto x_gone = Graph::null_vertex();
        if( !degree(x, g) ){
                cout << "no edges to x found, deleting\n";
                kill_vertex(x, g);
                x_gone = *vertices(g).first;
                cout << "x_gone: " << x_gone << '\n';
        } else {
                // delete all vertices x has replaced
                for( auto& v : replaceverts ) kill_vertex(v, g);
        }

        cout << HEADER_COL << "-------------------- 7 - New BFS and Make Max Planar -----\n" << RESET;
        reset_vertex_indices(g);
        reset_edge_index(g);
        vis_data.reset(&g);
        vis_data.root = (x_gone != Graph::null_vertex()) ? x_gone : x;
        ++vis_data.verts[vis_data.root].descendant_cost;

        cout << "root: " << vis_data.root << '\n';
        cout << "n:    " << num_vertices(g) << '\n';

        breadth_first_search(g, x_gone != Graph::null_vertex() ? x_gone: x, visitor(BFSVisitor(vis_data)));
        makemaxplanar(g);
        reset_vertex_indices(g);
        reset_edge_index(g);

        print_graph(g);

        cout << HEADER_COL << "----------------------- 8 - Locate Cycle -----------------\n" << RESET;
        auto chosen_edge = arbitrary_nontree_edge(g, vis_data);
        auto v1          = source(chosen_edge, g);
        auto w1          = target(chosen_edge, g);
        cout << "ancestors v1...\n";
        auto parents_v   = ancestors(v1, vis_data);
        cout << "ancestors v2...\n";
        auto parents_w   = ancestors(w1, vis_data);
        auto ancestor    = common_ancestor(parents_v, parents_w, vis_data);
        cout << "common ancestor: " << ancestor << '\n';
        auto cycle = get_cycle(v1, w1, ancestor, vis_data);

        Em   em2(&g);
        auto cc = compute_cycle_cost(cycle, g, vis_data, em2);
        if( cc.outside > cc.inside ){
                swap(cc.outside, cc.inside);
                cost_swapped = true;
                cout << "!!!!!! cost swapped !!!!!!!!\n";
        }
        cout << "total inside cost:  " << cc.inside  << '\n';
        cout << "total outside cost: " << cc.outside << '\n';

        cout << HEADER_COL << "---------------------------- 9 - Improve Separator -----------\n" << RESET;
        print_edges(g);

        while( cc.inside > num_vertices(g)*2./3 ){
                cout << RED << "chosen_edge: " << to_string(chosen_edge, g) << '\n';
                cout << "const inside: " << cc.inside  << '\n';
                cout << "const outide: " << cc.outside << '\n';
                cout << "looking for a better cycle\n" << RESET;

                auto vi = source(chosen_edge, g);
                auto wi = target(chosen_edge, g);
                assert(!vis_data.is_tree_edge(chosen_edge));
                EdgeDesc next_edge;
                cout << "   vi: " << vi << '\n';
                cout << "   wi: " << wi << '\n';

                auto neighbors_v = get_neighbors(vi, g);
                auto neighbors_w = get_neighbors(wi, g);
                auto intersect   = get_intersection(neighbors_v, neighbors_w);
                assert(intersect.size() == 2);
                cout << "   intersectbegin: " << *intersect.begin() << '\n';

                auto eee = edge(vi, *intersect.begin(), g);
                cout << "eee: " << to_string(eee.first, g) << '\n';
                assert(eee.second);

                InsideOut insideout = edge_inside_cycle(eee.first, *intersect.begin(), cycle, g, *em2.em);
                auto y = (insideout == INSIDE) ? *intersect.begin() : *(++intersect.begin());

                cout << "   y: " << y << '\n';
                auto viy_e = edge(vi, y, g); assert(viy_e.second); auto viy = viy_e.first;
                auto ywi_e = edge(y, wi, g); assert(ywi_e.second); auto ywi = ywi_e.first;
                if ( vis_data.is_tree_edge(viy) || vis_data.is_tree_edge(ywi) ){
                        cout << MAGENTA << "   at least one tree edge\n" << RESET;
                        next_edge = vis_data.is_tree_edge(viy) ? ywi : viy;
                        assert(!vis_data.is_tree_edge(next_edge));

                        // Compute the cost inside the (vi+1 wi+1) cycle from the cost inside the (vi, wi) cycle and the cost of vi, y, and wi.  See Fig 4.
                        uint cost1 = vis_data.verts[vi].descendant_cost;
                        uint cost2 = vis_data.verts[y ].descendant_cost;
                        uint cost3 = vis_data.verts[wi].descendant_cost;
                        uint cost4 = cc.inside;
                        auto new_cycle = get_cycle(source(next_edge, g), target(next_edge, g), vis_data);
                        cc = compute_cycle_cost(new_cycle, g, vis_data, em2); // !! CHEATED !!
                        if( cost_swapped ) swap(cc.outside, cc.inside);
                } else {
                        // Determine the tree path from y to the (vi, wi) cycle by following parent pointers from y.
                        cout << MAGENTA << "   neither are tree edges\n" << RESET;
                        auto path = ancestors(y, vis_data);
                        uint i;
                        for( i = 0; !on_cycle(path[i], cycle, g); ++i );

                        // Let z be the vertex on the (vi, wi) cycle reached during the search.
                        auto z = path[i++];
                        cout << "    z: " << z << '\n';
                        path.erase(path.begin()+i, path.end());
                        assert(path.size() == i);

                        // Compute the total cost af all vertices except z on this tree path.
                        uint path_cost = path.size() - 1;
                        cout << "    y-to-z-minus-z cost: " << path_cost << '\n';

                        // Scan the tree edges inside the (y, wi) cycle, alternately scanning an edge in one cycle and an edge in the other cycle.
                        // Stop scanning when all edges inside one of the cycles have been scanned.  Compute the cost inside this cycle by summing the associated costs of all scanned edges.
                        // Use this cost, the cost inside the (vi, wi) cycle, and the cost on the tree path from y to z to compute the cost inside the other cycle.
                        auto cycle1 = get_cycle(vi, y, vis_data);
                        auto cycle2 = get_cycle(y, wi, vis_data);

                        auto cost1  = compute_cycle_cost(cycle1, g, vis_data, em2);
                        auto cost2  = compute_cycle_cost(cycle2, g, vis_data, em2);
                        if( cost_swapped ){
                                swap(cost1.inside, cost1.outside);
                                swap(cost2.inside, cost2.outside);
                        }

                        // Let (vi+1, wi+1) be the edge among (vi, y) and (i, wi) whose cycle has more cost inside it.
                        if( cost1.inside > cost2.inside ){ next_edge = edge(vi, y, g).first; cc = cost1; }
                        else                             { next_edge = edge(y, wi, g).first; cc = cost2; }
                }
                chosen_edge = next_edge;
        }
        cout << "found cycle with inside cost < 2/3: " << cc.inside << '\n';
        print_cycle(cycle);

        cout << HEADER_COL << "\n------------ 10  - Construct Vertex Partition --------------\n" << RESET;
        print_graph(g_orig, false);
        cout << "l0: " << l[0] << '\n';
        cout << "l1: " << l[1] << '\n';
        cout << "l2: " << l[2] << '\n';

        uint r = vis_data.num_levels;
        cout << "r: " << r << '\n';

        if( l[1] >= l[2] ){
                cout << MAGENTA << "l1 is less than l2\n" << RESET;
                vector<VertDesc> part_a, part_b, part_c;
                VertIter vei, vend;
                for( tie(vei, vend) = vertices(g_orig); vei != vend; ++vei ){
                        auto v = *vei;
                        cout << "level of " << v << ": " << vis_data.verts[v].level << "  ";
                        if( vis_data.verts[v].level <  l[1] )                                  { cout << v << " belongs to first part\n";  part_a.push_back(v); continue; }
                        if( vis_data.verts[v].level >= l[1]+1 && vis_data.verts[v].level <= r ){ cout << v << " belongs to middle part\n"; part_b.push_back(v); continue; }
                        if( vis_data.verts[v].level == l[1] )                                  { cout << v << " belongs to last part\n";   part_c.push_back(v); continue; }
                        assert(0);
                }
                cout << GREEN;
                cout <<   "A = all verts on levels 0    thru l1-1: "; for( auto& a : part_a ) cout << a << ' ';
                cout << "\nB = all verts on levels l1+1 thru r   : "; for( auto& b : part_b ) cout << b << ' ';
                cout << "\nC = all verts on llevel l1            : "; for( auto& c : part_c ) cout << c << ' ';
                cout << RESET;
                return {};
        }

        vector<VertDesc> part_a, part_b, part_c, deleted_part;
        VertIter vei, vend;
        for( tie(vei, vend) = vertices(g_orig); vei != vend; ++vei ){
                auto v = *vei;
                cout << "level of " << v << ": " << vis_data.verts[v].level << ", ";
                if( vis_data.verts[v].level == l[1] || vis_data.verts[v].level == l[2] ){     cout << v << " is deleted\n";             deleted_part.push_back(v); continue;}
                if( vis_data.verts[v].level <  l[1] ){                                        cout << v << " belongs to first part\n";  part_a.push_back(v);       continue;}
                if( vis_data.verts[v].level >= l[1]+1 && vis_data.verts[v].level <= l[2]-1 ){ cout << v << " belongs to middle part\n"; part_b.push_back(v);       continue;}
                if( vis_data.verts[v].level >  l[2]  ){                                       cout << v << " belongs to last part\n";   part_c.push_back(v);       continue;}
                assert(0);
        }

        //the only part which can have cost > 2/3 is the middle part
        assert(part_a.size() <= 2*num_vertices(g_orig)/3);
        assert(part_c.size() <= 2*num_vertices(g_orig)/3);
        if( part_b.size() <= 2*num_vertices(g_orig)/3 ){
                cout << MAGENTA << "middle part NOT biggest\n" << RESET;
                vector<VertDesc>* costly_part, * other1, * other2;
                if( part_a.size() > part_b.size() && part_a.size() > part_c.size() ){ costly_part = &part_a; other1 = &part_b; other2 = &part_c; cout << "part a is most costly\n";}
                if( part_b.size() > part_a.size() && part_b.size() > part_c.size() ){ costly_part = &part_b; other1 = &part_a; other2 = &part_c; cout << "part b is most costly\n";}
                if( part_c.size() > part_a.size() && part_c.size() > part_b.size() ){ costly_part = &part_c; other1 = &part_a; other2 = &part_b; cout << "part c is most costly\n";}
                cout << "part a size: " << part_a.size() << '\n';
                cout << "part b size: " << part_b.size() << '\n';
                cout << "part c size: " << part_c.size() << '\n';
                cout <<   "A = most costly part of the 3: "; for( auto& a : *costly_part ) cout << a << ' ';
                cout << "\nB = remaining 2 parts        : "; for( auto& b : *other1      ) cout << b << ' '; for( auto& b : *other2 ) cout << b << ' ';
                cout << "\nC =                          : "; for( auto& v : deleted_part ) cout << v << ' '; cout << '\n';
        } else {
                cout << MAGENTA << "middle part biggest\n" << RESET;
                //delete all verts on level l2 and above
                //shrink all verts on levels l1 and belowe to a single vertex of cost zero
                //The new graph has a spanning tree radius of l2 - l1 -1 whose root corresponds to vertices on levels l1 and below in the original graph
                r = l[2] - l[1] - 1;
                //Apply Lemma 2 to the new graph, A* B* C*
                cout << "A = set among A* and B* with greater cost\n";
                cout << "C = verts on levels l1 and l2 in the original graph plus verts in C* minus the root\n";
                cout << "B = remaining verts\n";
                //By Lemma 2, A has total cost <= 2/3
                //But A U C* has total cost >= 1/3, so B also has total cost <= 2/3
                //Futhermore, C contains no more than L[l1] + L[l2] + 2(l2 - l1 - 1)
        }
        return {};
}