/**
 * @file dSFMTdc.cpp
 *
 * @brief The main function of parameter generator of dSFMT.
 *
 * The functions in this file are simple. They parse the command line
 * options and call all_in_one function which does almost all things.
 * Users can change this file so that it fits to their applications
 * and OS.
 *
 * @author Mutsuo Saito (Manieth corp.)
 * @author Makoto Matsumoto (Hiroshima University)
 *
 * Copyright (C) 2015 Mutsuo Saito, Makoto Matsumoto,
 * Hiroshima University.
 * All rights reserved.
 *
 * The 3-clause BSD License is applied to this software, see
 * LICENSE.txt
 */
#include <stdint.h>
#include <inttypes.h>
#include <time.h>
#include <fcntl.h>
#include <unistd.h>
#include <string>
#include <sstream>
#include <fstream>
#include <MTToolBox/AlgorithmReducibleRecursionSearch.hpp>
#include <MTToolBox/AlgorithmCalculateParity.hpp>
#include <MTToolBox/AlgorithmEquidistribution.hpp>
#include <MTToolBox/MersenneTwister64.hpp>
#include <NTL/GF2X.h>
#include <getopt.h>
#include "dSFMTsearch.hpp"
#include "AlgorithmDSFMTEquidistribution.hpp"
#include "Annihilate.h"
#include "calc_fixpoint.h"
#include "mpicontrol.hpp"

using namespace std;
using namespace MTToolBox;
using namespace NTL;

class options {
public:
    int mexp;
    bool verbose;
    int fixedSL1;
    int fixedPOS1;
    uint64_t seed;
    std::string filename;
    long count;
};

bool parse_opt(options& opt, int argc, char **argv);

int search(options& opt, ostream& os, int count);

/**
 * parse command line option, and search parameters
 * @param argc number of arguments
 * @param argv value of arguments
 * @return 0 if this ends normally
 */
int main(int argc, char** argv) {
    MPIControl mpi(&argc, &argv);
    options opt;
    bool parse = parse_opt(opt, argc, argv);
    if (!parse) {
        return -1;
    }
    ofstream ofs;
// MPI OUTPUT CHANGE
#if !defined(NO_MPI)
    // MPI では .txt を付けずに指定する。それが一番簡単
    opt.seed = opt.seed + mpi.getRank();
    if (!opt.filename.empty()) {
        char buff[200];
        sprintf(buff, ".s%04ld-%03d.txt", opt.seed, mpi.getRank());
        opt.filename += buff;
    }
#endif
    if (!opt.filename.empty()) {
        ofs.open(opt.filename.c_str());
        if (!ofs) {
            cerr << "can't open file:" << opt.filename << endl;
            return -1;
        }
        return search(opt, ofs, opt.count);
    } else {
        return search(opt, cout, opt.count);
    }
}

/**
 * search parameters using all_in_one function in the file search_all.hpp
 * @param opt command line options
 * @param count number of parameters user requested
 * @return 0 if this ends normally
 */
int search(options& opt, ostream& os, int count) {
    MersenneTwister64 mt(opt.seed);
    dSFMT g(opt.mexp);

    os << "seed = " << dec << opt.seed << endl;
    if (opt.verbose) {
        time_t t = time(NULL);
        os << "search start at " << ctime(&t);
    }
    if (opt.fixedSL1 > 0) {
        g.setFixedSL1(opt.fixedSL1);
    }
    if (opt.fixedPOS1 > 0) {
        g.setFixedPOS1(opt.fixedPOS1);
    }
    AlgorithmReducibleRecursionSearch<w128_t> ars(g, mt);
    int i = 0;
    AlgorithmCalculateParity<w128_t, dSFMT> cp;
    os << "# " << g.getHeaderString() << ", delta52"
         << endl;
    while (i < count) {
        if (ars.start(opt.mexp * 100)) {
            GF2X irreducible = ars.getIrreducibleFactor();
            GF2X characteristic = ars.getCharacteristicPolynomial();
            //os << "deg irreducible = " << dec << deg(irreducible) << endl;
            //os << "deg characteristic = " << dec << deg(characteristic)
            //     << endl;
            //os << "deg quotient = " << dec << deg(quotient) << endl;
            if (deg(irreducible) != opt.mexp) {
                os << "error" << endl;
                return -1;
            }
            getLCMPoly(characteristic, g);
            GF2X quotient = characteristic / irreducible;
            w128_t fixpoint = calc_fixpoint(g, irreducible, quotient);
            g.setFixPoint(fixpoint);
            cp.searchParity(g, irreducible);
            w128_t seed = {{1, 0, 0, 0}};
            g.seed(seed);
            if (!anni(g)) {
                return -1;
            }
            annihilate<w128_t>(&g, quotient);
            int veq52[52];
            DSFMTInfo info;
            info.bitSize = 128;
            info.elementNo = 2;
            int delta52
                = calc_dSFMT_equidistribution<w128_t, dSFMT>(g, veq52, 52, info,
                                                           opt.mexp);
            os << g.getParamString();
            os << dec << delta52 << endl;
            i++;
        } else {
            os << "search failed" << endl;
            break;
        }
    }
    if (opt.verbose) {
        time_t t = time(NULL);
        os << "search end at " << ctime(&t) << endl;
    }
    return 0;
}

static void output_help(string& pgm);

/**
 * command line option parser
 * @param opt a structure to keep the result of parsing
 * @param argc number of command line arguments
 * @param argv command line arguments
 * @param start default start value
 * @return command line options have error, or not
 */
bool parse_opt(options& opt, int argc, char **argv) {
    opt.verbose = false;
    opt.mexp = 0;
    opt.count = 1;
    opt.seed = (uint64_t)clock();
    opt.filename = "";
    opt.fixedSL1 = -1;
    opt.fixedPOS1 = -1;
    int c;
    bool error = false;
    string pgm = argv[0];
    static struct option longopts[] = {
        {"verbose", no_argument, NULL, 'v'},
        {"file", required_argument, NULL, 'f'},
        {"count", required_argument, NULL, 'c'},
        {"seed", required_argument, NULL, 's'},
        {"fixed-sl1", required_argument, NULL, 'x'},
        {"fixed-pos1", required_argument, NULL, 'X'},
        {NULL, 0, NULL, 0}};
    errno = 0;
    for (;;) {
        c = getopt_long(argc, argv, "vs:f:c:x:X:", longopts, NULL);
        if (error) {
            break;
        }
        if (c == -1) {
            break;
        }
        switch (c) {
        case 's':
            opt.seed = strtoull(optarg, NULL, 0);
            if (errno) {
                error = true;
                cerr << "seed must be a number" << endl;
            }
            break;
        case 'x':
            opt.fixedSL1 = strtoull(optarg, NULL, 0);
            if (errno) {
                error = true;
                cerr << "fixed sl1 must be a number" << endl;
            }
            break;
        case 'X':
            opt.fixedPOS1 = strtoull(optarg, NULL, 0);
            if (errno) {
                error = true;
                cerr << "fixed pos1 must be a number" << endl;
            }
            break;
        case 'v':
            opt.verbose = true;
            break;
        case 'f':
            opt.filename = optarg;
            break;
        case 'c':
            opt.count = strtoll(optarg, NULL, 10);
            if (errno) {
                error = true;
                cerr << "count must be a number" << endl;
            }
            break;
        case '?':
        default:
            error = true;
            break;
        }
    }
    argc -= optind;
    argv += optind;
    if (argc < 1) {
        error = true;
    } else {
        long mexp = strtol(argv[0], NULL, 10);
        static const int allowed_mexp[] = {521, 1279,
                                           2203, 4253,
                                           11213, 19937,
                                           44497, -1};
        if (! errno) {
            bool found = false;
            for (int i = 0; allowed_mexp[i] > 0; i++) {
                if (mexp == allowed_mexp[i]) {
                    found = true;
                    break;
                }
            }
            if (! found) {
                error = true;
            }
        }
        if (errno || error){
            error = true;
            cerr << "mexp must be one of ";
            for (int i = 0; allowed_mexp[i] > 0; i++) {
                cerr << dec << allowed_mexp[i] << " ";
            }
            cerr << endl;
        }
        opt.mexp = mexp;
    }
#if 0
    if (!opt.filename.empty()) {
        ofstream ofs(opt.filename.c_str());
        if (ofs) {
            ofs.close();
        } else {
            error = true;
            cerr << "can't open file:" << opt.filename << endl;
        }
    }
#endif
    if (error) {
        output_help(pgm);
        return false;
    }
    return true;
}

/**
 * showing help message
 * @param pgm program name
 */
static void output_help(string& pgm) {
    cerr << "usage:" << endl;
    cerr << pgm
         << " [-s seed] [-v] [-c count]"
         << " [-f outputfile]"
         << " mexp"
         << endl;
    static string help_string1 = "\n"
"--verbose, -v        Verbose mode. Output parameters, calculation time, etc.\n"
"--file, -f filename  Parameters are outputted to this file. without this\n"
"                     option, parameters are outputted to standard output.\n"
"--count, -c count    Output count. The number of parameters to be outputted.\n"
"--seed, -s seed      seed of randomness.\n"
"--fixed-sl1          fix the parameter sl1 to given value.\n"
"--fixed-pos1         fix the parameter pos1 to given value.\n"
"mexp                 mersenne exponent.\n"
        ;
    cerr << help_string1 << endl;
}


//==================================================================================================
/**
  Copyright 2016 NumScale SAS

  Distributed under the Boost Software License, Version 1.0.
  (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
**/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE1_SCALAR_FUNCTION_REC_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE1_SCALAR_FUNCTION_REC_HPP_INCLUDED

#include <boost/simd/detail/overload.hpp>
#include <boost/simd/function/refine_rec.hpp>
#include <boost/simd/function/raw.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( rec_
                          , (typename T)
                          , bs::sse_
                          , bs::raw_tag
                          , bd::scalar_<bd::single_<T>>
                          )
  {
    BOOST_FORCEINLINE T operator()(bs::raw_tag const&, T a0) const BOOST_NOEXCEPT
    {
      float inv;
      _mm_store_ss( &inv, _mm_rcp_ss( _mm_load_ss( &a0 ) ) );

      return inv;
    }
  };

} } }

#endif


//          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 <iostream>
#include <iomanip>
#include <boost/ptr_container/ptr_deque.hpp>
#include <boost/function.hpp>
#include <boost/timer/timer.hpp>
#include <boost/thread/thread.hpp>
#include <boost/bind.hpp>
#include <boost/format.hpp>
#include "unistd.h"
#include "config/config.hpp"
#if HAVE_CBLAS
	#include "cblas.h"
#endif

#include "Eigen/Core"
#include "Eigen/Eigenvalues"
#include "genfile/VariantIdentifyingData.hpp"
#include "genfile/VariantDataReader.hpp"
#include "genfile/SNPDataSourceProcessor.hpp"
#include "genfile/SingleSNPGenotypeProbabilities.hpp"
#include "genfile/vcf/get_set_eigen.hpp"
#include "statfile/BuiltInTypeStatSink.hpp"
#include "statfile/BuiltInTypeStatSource.hpp"
#include "worker/Task.hpp"
#include "worker/FunctionTask.hpp"
#include "appcontext/FileUtil.hpp"
#include "appcontext/get_current_time_as_string.hpp"
#include "components/RelatednessComponent/KinshipCoefficientComputer.hpp"
#include "components/RelatednessComponent/PCAComputer.hpp"
#include "components/RelatednessComponent/mean_centre_genotypes.hpp"

#include "boost/threadpool.hpp"

// #define DEBUG_KINSHIP_COEFFICIENT_COMPUTER 2
#define USING_BOOST_THREADPOOL 1

namespace impl {
	#if HAVE_EIGEN
		template< typename Vector, typename Matrix >
		void accumulate_xxt_using_eigen(
			Vector* data,
			Matrix* result,
			int const begin_sample_i, int const end_sample_i,
			int const begin_sample_j, int const end_sample_j,
			double const scale
		) {

			if( begin_sample_i == begin_sample_j && end_sample_i == end_sample_j ) {
				result
					->block( begin_sample_i, begin_sample_j, end_sample_i - begin_sample_i, end_sample_j - begin_sample_j )
					.template selfadjointView< Eigen::Lower >()
					.rankUpdate( data->segment( begin_sample_i, end_sample_i - begin_sample_i ), scale ) ;
			} else {
				result
					->block(  begin_sample_i, begin_sample_j, end_sample_i - begin_sample_i, end_sample_j - begin_sample_j ).noalias()
					+= scale * (
						data->segment( begin_sample_i, end_sample_i - begin_sample_i )
						*
						data->segment( begin_sample_j, end_sample_j - begin_sample_j ).transpose()
					) ;
			}
		}
	#endif
	#if HAVE_CBLAS
		void accumulate_xxt_using_cblas(
			KinshipCoefficientComputer::Computation::Vector* data,
			KinshipCoefficientComputer::Computation::Matrix* result,
			int const begin_sample_i, int const end_sample_i,
			int const begin_sample_j, int const end_sample_j,
			double const scale
		) {
			assert( result ) ;
			assert( data ) ;
			int const N = data->size() ;
			assert( N == result->cols() ) ;
			if( begin_sample_i == begin_sample_j && end_sample_i == end_sample_j ) {
				cblas_dsyr(
					CblasColMajor,
					CblasLower,
					end_sample_i - begin_sample_i,
					scale,
					data->segment( begin_sample_i, end_sample_i - begin_sample_i ).data(),
					1,
					result->block( begin_sample_i, begin_sample_j, end_sample_i - begin_sample_i, end_sample_j - begin_sample_j ).data(),
					result->outerStride()
				) ;
			} else {
				cblas_dger(
					CblasColMajor,
					end_sample_i - begin_sample_i,
					end_sample_j - begin_sample_j,
					scale,
					data->segment( begin_sample_i, end_sample_i - begin_sample_i ).data(), 1,
					data->segment( begin_sample_j, end_sample_j - begin_sample_j ).data(), 1,
					result->block( begin_sample_i, begin_sample_j, end_sample_i - begin_sample_i, end_sample_j - begin_sample_j ).data(),
					result->outerStride()
				) ;
			}
		}
	#endif

	std::ostream& operator<<( std::ostream& out, SampleBounds const& b ) {
		out << "[" << b.begin_sample_i << "-" << b.end_sample_i << ", " << b.begin_sample_j << "-" << b.end_sample_j << "]" ;
		return out ;
	}

	struct ComputeXXtTask: public boost::noncopyable
	{
		typedef KinshipCoefficientComputer::Computation::Vector Vector ;
		typedef KinshipCoefficientComputer::Computation::Matrix Matrix ;
		typedef KinshipCoefficientComputer::Computation::IntegerVector IntegerVector ;
		typedef KinshipCoefficientComputer::Computation::IntegerMatrix IntegerMatrix ;
		typedef std::auto_ptr< ComputeXXtTask > UniquePtr ;

		virtual ~ComputeXXtTask() {}

		virtual void add_data(
			KinshipCoefficientComputer::Computation::Vector&,
			KinshipCoefficientComputer::Computation::IntegerVector&
		) = 0 ;

		virtual void operator()() = 0 ;

		static UniquePtr create(
			Matrix* result,
			IntegerMatrix* counts,
			SampleBounds const bounds,
			std::string const& method
		) ;
	} ;

	struct ComputeXXtTaskImpl: public ComputeXXtTask {
		typedef boost::function< void (
			Vector* data,
			Matrix* result,
			int const begin_sample_i, int const end_sample_i,
			int const begin_sample_j, int const end_sample_j,
			double const scale
		) >
		AccumulateXXt ;

		typedef boost::function< void (
			IntegerVector* data,
			IntegerMatrix* result,
			int const begin_sample_i, int const end_sample_i,
			int const begin_sample_j, int const end_sample_j,
			double const scale
		) >
		AccumulateXXtInteger ;

		ComputeXXtTaskImpl(
			Matrix* result,
			IntegerMatrix* counts,
			SampleBounds const bounds,
			AccumulateXXt accumulate_xxt,
			AccumulateXXtInteger accumulate_xxt_integer
		):
			m_result( result ),
			m_counts( counts ),
			m_bounds( bounds ),
			m_accumulate_xxt( accumulate_xxt ),
			m_accumulate_xxt_integer( accumulate_xxt_integer )
		{
			assert( result ) ;
			assert( m_accumulate_xxt ) ;
		}

		void add_data(
			Vector& data,
			IntegerVector& nonmissingness
		) {
			m_data.push_back( &data ) ;
			m_nonmissingness.push_back( &nonmissingness ) ;
		}

		std::size_t number_of_snps() const { return m_data.size() ; }

		void operator()() {
			for( std::size_t snp_i = 0; snp_i < m_data.size(); ++snp_i ) {
				m_accumulate_xxt(
					m_data[ snp_i ],
					m_result,
					m_bounds.begin_sample_i, m_bounds.end_sample_i,
					m_bounds.begin_sample_j, m_bounds.end_sample_j,
					1.0
				) ;

				m_accumulate_xxt_integer(
					m_nonmissingness[ snp_i ],
					m_counts,
					m_bounds.begin_sample_i, m_bounds.end_sample_i,
					m_bounds.begin_sample_j, m_bounds.end_sample_j,
					1.0
				) ;
			}
		}

	protected:
		Matrix* m_result ;
		IntegerMatrix* m_counts ;
		SampleBounds const m_bounds ;
		std::vector< Vector* > m_data ;
		std::vector< IntegerVector* > m_nonmissingness ;
		AccumulateXXt m_accumulate_xxt ;
		AccumulateXXtInteger m_accumulate_xxt_integer ;
	} ;

	struct ComputeXXtUsingEigenTask: public ComputeXXtTaskImpl {
		ComputeXXtUsingEigenTask(
			Matrix* result,
			IntegerMatrix* counts,
			SampleBounds const bounds
		):
			ComputeXXtTaskImpl(
				result, counts, bounds,
				&impl::accumulate_xxt_using_eigen< Vector, Matrix >, &impl::accumulate_xxt_using_eigen< IntegerVector, IntegerMatrix >
			)
		{}
	} ;

#if HAVE_CBLAS
	struct ComputeXXtUsingCBlasTask: public ComputeXXtTaskImpl {
		ComputeXXtUsingCBlasTask(
			Matrix* result,
			IntegerMatrix* counts,
			SampleBounds const bounds
		):
			ComputeXXtTaskImpl(
				result, counts, bounds,
				&impl::accumulate_xxt_using_cblas, &impl::accumulate_xxt_using_eigen< IntegerVector, IntegerMatrix >
			)
		{}
	} ;
#endif

	ComputeXXtTask::UniquePtr ComputeXXtTask::create(
		Matrix* result,
		IntegerMatrix* counts,
		SampleBounds const bounds,
		std::string const& method
	) {
#if HAVE_CBLAS
		if( method == "cblas" ) {
			return ComputeXXtTask::UniquePtr(
				new ComputeXXtUsingCBlasTask(
					result, counts, bounds
				)
			) ;
		} else
#endif
		 if( method == "eigen" ) {
			return ComputeXXtTask::UniquePtr(
				new ComputeXXtUsingEigenTask(
					result, counts, bounds
				)
			) ;
		} else {
			assert(0) ;
		}
	}

	struct Dispatcher {
		typedef std::auto_ptr< Dispatcher > UniquePtr ;
		typedef KinshipCoefficientComputer::Computation::Vector Vector ;
		typedef KinshipCoefficientComputer::Computation::Matrix Matrix ;
		typedef KinshipCoefficientComputer::Computation::IntegerVector IntegerVector ;
		typedef KinshipCoefficientComputer::Computation::IntegerMatrix IntegerMatrix ;
		typedef boost::function<
			ComputeXXtTask::UniquePtr ( Matrix*, IntegerMatrix*, SampleBounds const )
		> TaskFactory ;

		virtual ~Dispatcher() {}
		virtual void setup( std::size_t number_of_samples ) = 0 ;
		virtual void get_storage( Vector* data, IntegerVector* nonmissingness ) = 0 ;
		virtual void add_data( Vector* data, IntegerVector* nonmissingness ) = 0 ;
		virtual void finalise() = 0 ;
		virtual void wait_until_complete() = 0 ;
	} ;

	//
	// Return a tiling of a dxd matrix into (roughly) B blocks.
	// Blocks should be roughly evenly sized.
	// This implementation makes off-diagonal blocks half the size of diagonal blocks.
	std::vector< SampleBounds > get_matrix_lower_diagonal_tiling( std::size_t d, std::size_t B ) {
		// Because off-diagonal blocks involve about twice as much computation,
		// we make them half the size vertically.
		// If there are (N ( N+1)/2 ) square blocks this makes
		// (N (N+1) ) - N = N^2 blocks in total.
		// So we want to choose N so that N^2 is about twice the number of threads.
		// Def
		std::size_t const N = B ;
		std::vector< SampleBounds > result ;
		double K = double( d ) / N ;
//#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
		std::cerr << "get_matrix_lower_diagonal_tiling: " << d << "x" << d << " matrix, "
			<< N << " blocks across diagonal, "
			<< N*(N-1) << " blocks in lower triangle.\n" ;
//#endif
		// Set up some initial tasks.
		// Do j (the column) as the outer loop
		// so that tasks go in column-major direction.
		for( std::size_t j = 0; j < N; ++j ) {
			for( std::size_t i = 0; i < N; ++i ) {
				if( i == j ) {
					SampleBounds bounds ;
					bounds.begin_sample_i = std::ceil(i*K) ;
					bounds.end_sample_i = std::min( std::size_t( std::ceil( (i+1)*K ) ), d ) ;
					bounds.begin_sample_j = bounds.begin_sample_i ;
					bounds.end_sample_j = bounds.end_sample_i ;
					result.push_back( bounds ) ;
				}
				else {
					// Do two vertical stripes
					SampleBounds bounds ;
					bounds.begin_sample_i = std::ceil(i*K) ;
					bounds.end_sample_i = std::min( std::size_t( std::ceil( (i+1)*K ) ), d ) ;
					bounds.begin_sample_j = std::ceil(j*K) ;
					bounds.end_sample_j = std::ceil((j+0.5)*K) ;
					result.push_back( bounds ) ;

					bounds.begin_sample_j = bounds.end_sample_j ;
					bounds.end_sample_j = std::min( std::size_t( std::ceil( (j+1)*K ) ), d ) ;
					result.push_back( bounds ) ;
				}
			}
		}
//#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
		std::cerr << "get_matrix_lower_diagonal_tiling: tiling has " << result.size() << " blocks in total.\n" ;
//#endif
		return result ;
	}

	std::vector< SampleBounds > get_matrix_lower_diagonal_vertical_strip_tiling( std::size_t d, std::size_t const strip_width ) {
		std::vector< SampleBounds > result ;
//#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
		std::cerr << "get_matrix_lower_diagonal_vertical_strip_tiling: " << d << "x" << d << " matrix" ;
//#endif
		// Set up some initial tasks.
		// Do j (the column) as the outer loop
		// so that tasks go in column-major direction.
		for( std::size_t i = 0; i < d; i += strip_width ) {
			SampleBounds bounds ;
			bounds.begin_sample_i = i ;
			bounds.end_sample_i = d ;
			bounds.begin_sample_j = i ;
			bounds.end_sample_j = std::min( i+strip_width, d ) ;
			result.push_back( bounds ) ;
		}
//#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
		std::cerr << "get_matrix_lower_diagonal_vertical_strip_tiling: tiling has " << result.size() << " blocks in total.\n" ;
//#endif
		return result ;
	}

	std::vector< SampleBounds > get_matrix_lower_diagonal_horizontal_strip_tiling( std::size_t d, std::size_t const strip_width ) {
		std::vector< SampleBounds > result ;
//#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
		std::cerr << "get_matrix_lower_diagonal_horizontal_strip_tiling: " << d << "x" << d << " matrix" ;
//#endif
		// Set up some initial tasks.
		// Do j (the column) as the outer loop
		// so that tasks go in column-major direction.
		for( std::size_t i = 0; i < d; i += strip_width ) {
			SampleBounds bounds ;
			bounds.begin_sample_i = i ;
			bounds.end_sample_i = i + strip_width ;
			bounds.begin_sample_j = 0 ;
			bounds.end_sample_j = std::min( i+strip_width, d ) ;
			result.push_back( bounds ) ;
		}
//#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
		std::cerr << "get_matrix_lower_diagonal_horizontal_strip_tiling: tiling has " << result.size() << " blocks in total.\n" ;
//#endif
		return result ;
	}
	struct MultiThreadedDispatcher: public Dispatcher {
		typedef void (*accumulate_xxt_t)( Vector*, Matrix*, int const begin_sample_i, int const end_sample_i, int const begin_sample_j, int const end_sample_j, double const ) ;
		typedef void (*accumulate_xxt_integer_t)( IntegerVector*, IntegerMatrix*, int const begin_sample_i, int const end_sample_i, int const begin_sample_j, int const end_sample_j, double const ) ;

		MultiThreadedDispatcher(
			Matrix& result,
			IntegerMatrix& nonmissingness,
			worker::Worker* worker,
			TaskFactory task_factory
		):
			m_result( result ),
			m_nonmissingness( nonmissingness ),
			m_worker( worker ),
			m_task_factory( task_factory ),
			m_storage( 100, std::make_pair( Vector(), IntegerVector() ) ),
			m_storage_index( 0 ),
			m_data_index( 0 )
#if USING_BOOST_THREADPOOL
			,m_pool( m_worker->get_number_of_worker_threads() )
#endif
		{
		}

		~MultiThreadedDispatcher() {
		}

		void setup( std::size_t number_of_samples ) {
			// If there are N worker threads go for about two jobs per thread.
			std::size_t numberOfTasks = 2 * std::sqrt( m_worker->get_number_of_worker_threads() ) ;

			m_bounds = get_matrix_lower_diagonal_tiling( number_of_samples, numberOfTasks ) ;
			for( std::size_t i = 0; i < m_bounds.size(); ++i ) {
				m_tasks.push_back( 0 ) ;
			}
		}

		void get_storage( Vector* data, IntegerVector* nonmissingness ) {
			data->swap( m_storage[ m_storage_index % m_storage.size() ].first ) ;
			nonmissingness->swap( m_storage[ m_storage_index % m_storage.size() ].second ) ;
		}

		void add_data( Vector* data, IntegerVector* nonmissingness ) {
			// Take the data (without copying it).
			data->swap( m_storage[ m_storage_index % m_storage.size() ].first ) ;
			nonmissingness->swap( m_storage[ m_storage_index % m_storage.size() ].second ) ;

			// Add 20 SNPs at a time before submitting.
			bool const schedule = ( m_data_index % 20 ) == 0 ;
			++m_data_index ;

			for( std::size_t task_index = 0; task_index < m_bounds.size(); ++task_index ) {
				std::size_t const bound_index = task_index ;

				if( m_tasks.is_null( task_index ) ) {
					ComputeXXtTask::UniquePtr new_task = m_task_factory(
						&m_result,
						&m_nonmissingness,
						m_bounds[ bound_index ]
					) ;
					m_tasks.replace( task_index, new_task ) ;
				}
				m_tasks[ task_index ].add_data(
					m_storage[ m_storage_index % m_storage.size() ].first,
					m_storage[ m_storage_index % m_storage.size() ].second
				) ;

				if( schedule ) {
					m_pool.schedule( boost::bind( &ComputeXXtTask::operator(), &m_tasks[ task_index ] )) ;
				}
			}
			if( schedule ) {
				m_pool.wait() ;
				for( std::size_t i = 0; i < m_tasks.size(); ++i ) {
					m_tasks.replace( i, 0 ) ;
				}
			}

			++m_storage_index ;
		}

		void finalise() {
			if( m_storage_index % m_storage.size() > 0 ) {
				// we have unprocessed data.
				for( std::size_t task_index = 0; task_index < m_bounds.size(); ++task_index ) {
					m_pool.schedule( boost::bind( &ComputeXXtTask::operator(), &m_tasks[ task_index ] )) ;
				}
				m_pool.wait() ;
				for( std::size_t i = 0; i < m_tasks.size(); ++i ) {
					m_tasks.replace( i, 0 ) ;
				}
			}
		}

		void wait_until_complete() {
		}

	private:
		Matrix& m_result ;
		IntegerMatrix& m_nonmissingness ;
		worker::Worker* m_worker ;
		TaskFactory m_task_factory ;
		std::vector< SampleBounds > m_bounds ;
		boost::ptr_deque< boost::nullable< ComputeXXtTask > > m_tasks ;
		std::vector<
			std::pair<
				KinshipCoefficientComputer::Computation::Vector,
				KinshipCoefficientComputer::Computation::IntegerVector
			>
		> m_storage ;
		std::size_t m_storage_index ;
		std::size_t m_data_index ;
#if USING_BOOST_THREADPOOL
		boost::threadpool::pool m_pool ;
#endif
	} ;

	NormaliseGenotypesAndComputeXXt::UniquePtr NormaliseGenotypesAndComputeXXt::create( worker::Worker* worker, std::string const& method ) {
		return NormaliseGenotypesAndComputeXXt::UniquePtr( new NormaliseGenotypesAndComputeXXt( worker, method) ) ;
	}

	NormaliseGenotypesAndComputeXXt::NormaliseGenotypesAndComputeXXt(
		worker::Worker* worker,
		std::string const& method
	):
		m_call_threshhold( 0.9 ),
		m_allele_frequency_threshhold( 0.001 ),
		m_number_of_snps_included( 0 ),
		m_dispatcher(
			new impl::MultiThreadedDispatcher(
				m_result,
				m_nonmissingness,
				worker,
				boost::bind(
					&ComputeXXtTask::create,
					_1, _2, _3, method
				)
			)
		)
	{}

	KinshipCoefficientComputer::Computation::Matrix const& NormaliseGenotypesAndComputeXXt::result() const { return m_result ; }
	KinshipCoefficientComputer::Computation::IntegerMatrix const& NormaliseGenotypesAndComputeXXt::nonmissingness() const { return m_nonmissingness ; }

	std::string NormaliseGenotypesAndComputeXXt::get_summary() const {
		return (
			boost::format( "NormaliseGenotypesAndComputeXXt( min allele frequency %.3f)" )
			% m_allele_frequency_threshhold
		).str()
		;
	}

	void NormaliseGenotypesAndComputeXXt::begin_processing_snps( std::size_t number_of_samples, genfile::SNPDataSource::Metadata const& ) {
		m_result.setZero( number_of_samples, number_of_samples ) ;
		m_nonmissingness.setZero( number_of_samples, number_of_samples ) ;
		m_dispatcher->setup( number_of_samples ) ;
	}

	void NormaliseGenotypesAndComputeXXt::processed_snp(
		genfile::VariantIdentifyingData const& id_data,
		genfile::VariantDataReader::SharedPtr data_reader
	) {
		KinshipCoefficientComputer::Computation::Vector genotypes ;
		KinshipCoefficientComputer::Computation::IntegerVector nonmissingness ;

		m_dispatcher->get_storage( &genotypes, &nonmissingness ) ;

		genotypes.setZero( m_result.rows() ) ;
		nonmissingness.setZero( m_result.rows() ) ;

		data_reader->get(
			":genotypes:",
			genfile::vcf::get_threshholded_calls( genotypes, nonmissingness, m_call_threshhold, 0, 0, 1, 2 )
		) ;

		// estimate allele frequency using data augmentation:
		// add two haplotypes, one with each allele.
		// This prevents the estimate becoming one or zero.
		double const posterior_allele_frequency = (1 + genotypes.sum()) / ( 2 + (2.0 * nonmissingness.sum()) ) ;
		double const allele_frequency = genotypes.sum() / (2.0 * nonmissingness.sum()) ;
		if( std::min( allele_frequency, 1.0 - allele_frequency ) > m_allele_frequency_threshhold ) {
			pca::mean_centre_genotypes( &genotypes, nonmissingness, allele_frequency ) ;
			// Could compute actual SNP sd:
			//double const sd = std::sqrt( ( genotypes.array().square().sum() - ( genotypes.sum() * genotypes.sum() ) ) / ( nonmissingness.sum() - 1 ) ) ;
			// Or sd based on allele frequency:
			double const sd = std::sqrt( ( 2.0 * posterior_allele_frequency * ( 1.0 - posterior_allele_frequency )) ) ;
			genotypes /= sd ;
			m_dispatcher->add_data( &genotypes, &nonmissingness ) ;
			++m_number_of_snps_included ;
		}
	}

	void NormaliseGenotypesAndComputeXXt::end_processing_snps() {
		m_dispatcher->finalise() ;
		m_result.array() /= m_nonmissingness.array().cast< double >() ;
	}

	std::size_t NormaliseGenotypesAndComputeXXt::number_of_snps_included() const {
		return m_number_of_snps_included ;
	}
}

namespace impl {
	NTaskDispatcher::NTaskDispatcher(
		worker::Worker* worker
	):
		m_worker( worker )
	{
	}

	void NTaskDispatcher::set_number_of_tasks( std::size_t number_of_tasks ) {
		wait_until_complete() ;
		// TODO: do this better using the constructor
		// I don't have the docs here so don't know the right call.
		m_tasks.clear() ;
		for( std::size_t i = 0; i < number_of_tasks; ++i ) {
			m_tasks.push_back( 0 ) ;
		}
	}

	std::size_t NTaskDispatcher::number_of_tasks() const { return m_tasks.size() ; }

	void NTaskDispatcher::submit_task( std::size_t task_i, boost::function< void() > task ) {
		assert( task_i < m_tasks.size() ) ;
		if( !m_tasks.is_null( task_i ) ) {
			throw genfile::BadArgumentError(
				"impl::NTaskDispatcher::submit_task()",
				( boost::format( "task_i=%d" ) % task_i ).str(),
				"Task is still running.  Call wait_until_complete() first."
			) ;
		}
		worker::Task::UniquePtr new_task( new worker::FunctionTask( task ) ) ;
		m_tasks.replace( task_i, new_task ) ;
		m_worker->tell_to_perform_task( m_tasks[ task_i ] ) ;
	}

	void NTaskDispatcher::wait_until_complete() {
		for( std::size_t i = 0; i < m_tasks.size(); ++i ) {
			if( !m_tasks.is_null( i ) ) {
				m_tasks[ i ].wait_until_complete() ;
				m_tasks.replace( i, 0 ) ;
			}
		}
	}

	NormaliseGenotypesAndComputeXXtFast::UniquePtr NormaliseGenotypesAndComputeXXtFast::create(
		worker::Worker* worker,
		std::size_t const number_of_snps_per_computation
	) {
		return NormaliseGenotypesAndComputeXXtFast::UniquePtr( new NormaliseGenotypesAndComputeXXtFast( worker, number_of_snps_per_computation ) ) ;
	}

	NormaliseGenotypesAndComputeXXtFast::NormaliseGenotypesAndComputeXXtFast(
		worker::Worker* worker,
		std::size_t const number_of_snps_per_chunk
	):
		m_worker( worker ),
		m_dispatcher( new NTaskDispatcher( worker ) ),
		m_call_threshhold( 0.9 ),
		m_allele_frequency_threshhold( 0.001 ),
		m_number_of_snps_per_chunk( number_of_snps_per_chunk ),
		m_number_of_snps_per_computation( 4 * number_of_snps_per_chunk )
	{
		// We allocate lookup tables exactly once, here.
		// We follow, roughly, the scheme in plink 1.9 of having four lookup tables.
		// Here we encode pairs of genotypes in 4 bits (though we could do 3 as in plink).
		// Each computation then involves 16 variants, four from each lookup table.
		// This multi-table version reduces the number of times the result matrices
		// have to be traversed, and seems to make most difference in speed when
		// multithreading.
		m_lookup_tables.resize( 4, std::vector< double >( 1 << ( m_number_of_snps_per_chunk * 4 ), 0.0 ) ) ;
		m_nonmissingness_lookup_tables.resize( 4, std::vector< int >( 1 << ( m_number_of_snps_per_chunk * 4 ), 0.0 ) ) ;
	}

	NormaliseGenotypesAndComputeXXtFast::Matrix const& NormaliseGenotypesAndComputeXXtFast::result() const {
		return m_result ;
	}

	NormaliseGenotypesAndComputeXXtFast::IntegerMatrix const& NormaliseGenotypesAndComputeXXtFast::nonmissingness() const {
		return m_nonmissingness ;
	}

	std::size_t NormaliseGenotypesAndComputeXXtFast::number_of_snps_included() const {
		return m_snp_count ;
	}

	std::string NormaliseGenotypesAndComputeXXtFast::get_summary() const {
		return (
			boost::format(
				"NormaliseGenotypesAndComputeXXtFast():\n"
				" - %d SNPs per chunk\n"
				" - %d chunks per array acces\n"
				" - four lookup tables of size: %d\n"
				" - minimum allele frequency: %.3f"
			)
				% m_number_of_snps_per_chunk
				% (m_number_of_snps_per_computation / m_number_of_snps_per_chunk)
				% m_lookup_tables[0].size()
				% m_allele_frequency_threshhold
		).str()
		;
	}
	void NormaliseGenotypesAndComputeXXtFast::begin_processing_snps(
		std::size_t number_of_samples,
		genfile::SNPDataSource::Metadata const&
	) {
		m_result.resize( number_of_samples, number_of_samples ) ;
		m_result.setZero() ;
		m_nonmissingness.resize( number_of_samples, number_of_samples ) ;
		m_nonmissingness.setZero() ;
		std::size_t numberOfTasks = m_worker->get_number_of_worker_threads() ;
#if 0
		m_matrix_tiling = get_matrix_lower_diagonal_vertical_strip_tiling( number_of_samples, 1 ) ;
#else
		m_matrix_tiling = get_matrix_lower_diagonal_tiling(
			number_of_samples,
			numberOfTasks
		) ;
#endif
		m_dispatcher->set_number_of_tasks( m_matrix_tiling.size() ) ;
		m_combined_genotypes.resize( number_of_samples, 0 ) ;
		m_per_snp_genotypes.resize( number_of_samples, 0 ) ;
		m_snp_count = 0 ;
	}

	namespace impl {
		template< typename T >
		double add( T a, T b ) {
			return a + b ;
		}
	}

#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
	namespace {
		template< typename T >
		void print_lookup_table( std::ostream& out, std::vector< T > const& lookup_table, std::size_t const max = 64 ) {
			out << "print_lookup_table(): lookup table of size " << lookup_table.size() << ":" ;
			for( std::size_t i = 0; i < std::min( max, lookup_table.size() ); ++i ) {
				if( i % 8 == 0 ) {
					out << "\n" ;
				} else {
					out << " " ;
				}
				out << ( boost::format( " - 0x%02x: %.2f" ) % i % lookup_table[i] ) ;
			}
			out << "\n" ;
		}
	}
#endif

	void NormaliseGenotypesAndComputeXXtFast::processed_snp(
		genfile::VariantIdentifyingData const& id_data,
		genfile::VariantDataReader::SharedPtr data_reader
	) {
		// std::vector< std::size_t > m_genotypes
		// std::vector< int >
		m_per_snp_genotypes.assign( m_result.rows(), 0 ) ;

		// I find it simplest here to encode genotypes as
		// 0 (missing), 1 (AA homozygote), 2 (heterozygote), 3 (BB homozygote).
		data_reader->get(
			":genotypes:",
			genfile::vcf::get_threshholded_calls( m_per_snp_genotypes, m_call_threshhold, 0, 1, 2, 3 )
		) ;

		// compute allele frequency
		double allele2_count = 0.0 ;
		double nonmissing_count = 0.0 ;
		for( std::size_t i = 0; i < m_combined_genotypes.size(); ++i ) {
			bool const missing = ( m_per_snp_genotypes[i] == 0 ) ;
			if( !missing ) {
				allele2_count += m_per_snp_genotypes[i] - 1.0 ;
				nonmissing_count += 1 ;
			}
		}
		// Data estimate:
		// double const allele_frequency = allele2_count / ( nonmissing_count * 2.0 ) ;
		// Regularised estimate based on adding one of each allele,
		// this matches Price et al Nature Genetics 2006
		double const posterior_allele_frequency = (1 + allele2_count) / ( 2 + ( 2 * nonmissing_count)) ;
		double const allele_frequency = allele2_count / ( 2 * nonmissing_count ) ;
		if( nonmissing_count > 0 && std::min( allele_frequency, 1.0 - allele_frequency ) > m_allele_frequency_threshhold ) {
			// Ok we will process this SNP.
			// We batch SNPs together to form a total of m_number_of_snps_per_computation SNPs.
			// This means we visit the result matrix (which for large samples won't fit in cache)
			// as few times as possible.
			// Here we are not quite as efficient as plink: I use four bits instead of three to
			// encode the two genotypes.
			std::size_t const lookup_snp_index = m_snp_count % m_number_of_snps_per_computation ;
			if( lookup_snp_index == 0 ) {
				m_dispatcher->wait_until_complete() ;
				std::copy( m_per_snp_genotypes.begin(), m_per_snp_genotypes.end(), m_combined_genotypes.begin() ) ;
				// Clear the lookup tables
				for( std::size_t i = 0; i < m_lookup_tables.size(); ++i ) {
					std::fill( m_lookup_tables[i].begin(), m_lookup_tables[i].end(), 0.0 ) ;
					std::fill( m_nonmissingness_lookup_tables[i].begin(), m_nonmissingness_lookup_tables[i].end(), 0.0 ) ;
				}
			} else {
				// We allow 4 bits per genotype, although each genotype takes up only the lower two bits.
				// This allows us to encode pairs of genotypes in consecutive pairs of bits
				// for simpler lookup of the lookup table.
				for( std::size_t i = 0; i < m_combined_genotypes.size(); ++i ) {
					m_combined_genotypes[i] |= m_per_snp_genotypes[i] << (4 * lookup_snp_index) ;
				}
			}

			std::size_t const lookup_table_index = ( m_snp_count % m_number_of_snps_per_computation ) / m_number_of_snps_per_chunk ;
			double const mean = 2.0 * allele_frequency ;
			double const sd = std::sqrt( ( 2.0 * posterior_allele_frequency * ( 1.0 - posterior_allele_frequency )) ) ;
#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
			std::cerr << "NormaliseGenotypesAndComputeXXtFast::processed_snp(): filling lookup table " << lookup_table_index << ".\n" ;
#endif

			add_snp_to_lookup_table(
				lookup_snp_index % m_number_of_snps_per_chunk,
				mean, sd,
				&m_lookup_tables[ lookup_table_index ],
				&m_nonmissingness_lookup_tables[ lookup_table_index ]
			) ;

			++m_snp_count ;
#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
			boost::format fmt( "%.2f" ) ;
			std::cerr << "NormaliseGenotypesAndComputeXXtFast::processed_snp(): m_snp_count: " << m_snp_count << ".\n" ;
			std::cerr << "NormaliseGenotypesAndComputeXXtFast::processed_snp(): allele_frequency: " << fmt % allele_frequency << ".\n" ;
			std::cerr << "NormaliseGenotypesAndComputeXXtFast::processed_snp(): mean: " << fmt % mean << ", sd = " << fmt % sd << ".\n" ;
			print_lookup_table( std::cerr, m_lookup_tables[0], 32 ) ;
			print_lookup_table( std::cerr, m_lookup_tables[1], 32 ) ;
			print_lookup_table( std::cerr, m_lookup_tables[2], 32 ) ;
			print_lookup_table( std::cerr, m_lookup_tables[3], 32 ) ;
			//print_lookup_table( std::cerr, m_nonmissingness_lookup_table, 256 ) ;
#endif
			if( m_snp_count % m_number_of_snps_per_computation == 0 ) {
#if DEBUG_KINSHIP_COEFFICIENT_COMPUTER
			std::cerr << "NormaliseGenotypesAndComputeXXtFast::processed_snp(): submitting tasks...\n" ;
#endif
				submit_tasks( m_combined_genotypes ) ;
			}
		}
	}

	void NormaliseGenotypesAndComputeXXtFast::add_snp_to_lookup_table(
		std::size_t const snp_lookup_index,
		double const mean,
		double const sd,
		std::vector< double >* lookup_table,
		std::vector< int >* nonmissingness_lookup_table
	) {
		// We lay out the lookup table as follows.
		// The two genotypes at the first SNP occupy the first 4 bits,
		// the two genotypes at the second SNP occupy the next 4 bits,
		// etc.
		// Thus after k SNPs the size of the useful part of the table is 16^k,
		// which is computed as (1 << 4k).
		// Note that because genotype value 0 corresponds to missing data,
		// The block as computed after SNP k-1 is equal to the block
		// for missing values for SNP k.
		if( snp_lookup_index == 0 ) {
			// bottom 2 bits correspond to top genotype = 0.
			for( std::size_t g = 0; g < 4; ++g ) {
				(*nonmissingness_lookup_table)[g] = 0 ;
				(*lookup_table)[ g ] = 0 ;
			}
			for( std::size_t g = 1; g < 4; ++g ) {
				(*nonmissingness_lookup_table)[ (g << 2) + 0 ] = 0 ;
				(*nonmissingness_lookup_table)[ (g << 2) + 1 ] = 1 ;
				(*nonmissingness_lookup_table)[ (g << 2) + 2 ] = 1 ;
				(*nonmissingness_lookup_table)[ (g << 2) + 3 ] = 1 ;
				(*lookup_table)[ (g << 2) + 0 ] = 0.0 ;
				(*lookup_table)[ (g << 2) + 1 ] = ( ( 0.0 - mean ) / sd ) * ( ( (g-1) - mean ) / sd ) ;
				(*lookup_table)[ (g << 2) + 2 ] = ( ( 1.0 - mean ) / sd ) * ( ( (g-1) - mean ) / sd ) ;
				(*lookup_table)[ (g << 2) + 3 ] = ( ( 2.0 - mean ) / sd ) * ( ( (g-1) - mean ) / sd ) ;
			}
		} else {
			// We have already computed the initial block of size 16^k.
			// Now we compute 15 additional blocks for 16 in total.
			// In the computation we break these up as 4 blocks of size
			// This means this SNP has no effect and we need only copy.
			std::size_t const inner_block_size = 1 << ( 4 * snp_lookup_index ) ;
			std::size_t const outer_block_size = 4 * inner_block_size ;
			for( std::size_t outer_g = 0; outer_g < 4; ++outer_g ) {
				for( std::size_t inner_g = (outer_g == 0) ? 1 : 0; inner_g < 4; ++inner_g ) {
					double const value = ( inner_g == 0 || outer_g == 0 )
						? 0.0
						: (((inner_g-1.0) - mean)/sd) * (((outer_g-1.0) - mean)/sd) ;
					int const nonmissingness = ( inner_g == 0 || outer_g == 0 )
						? 0
						: 1 ;
					std::transform(
						lookup_table->begin(),
						lookup_table->begin() + inner_block_size,
						lookup_table->begin() + outer_block_size * outer_g + inner_block_size * inner_g,
						boost::bind( &impl::add< double >, _1, value )
					) ;
					std::transform(
						nonmissingness_lookup_table->begin(),
						nonmissingness_lookup_table->begin() + inner_block_size,
						nonmissingness_lookup_table->begin() + outer_block_size * outer_g + inner_block_size * inner_g,
						boost::bind( &impl::add< int >, _1, nonmissingness )
					) ;
				}
			}
		}
	}

	void NormaliseGenotypesAndComputeXXtFast::submit_tasks(
		std::vector< uint64_t > const& genotypes
	) {
		for( std::size_t tile_i = 0; tile_i < m_matrix_tiling.size(); ++tile_i ) {
	#if 1
			m_dispatcher->submit_task(
				tile_i,
				boost::bind(
					&NormaliseGenotypesAndComputeXXtFast::compute_block,
					this,
					boost::cref( m_matrix_tiling[ tile_i ] ),
					boost::cref( genotypes )
				)
			) ;
	#else
			compute_block( m_matrix_tiling[tile_i], genotypes, lookup_table, nonmissingness_lookup_table ) ;
	#endif
		}
	}

	void NormaliseGenotypesAndComputeXXtFast::end_processing_snps() {
		if( m_snp_count % m_number_of_snps_per_computation > 0 ) {
			submit_tasks( m_combined_genotypes ) ;
		}
		m_dispatcher->wait_until_complete() ;
		std::cerr << "snp count is: " << m_snp_count << ".\n" ;
		std::cerr << "non-missingness array is:\n" << m_nonmissingness.block(0,0,10,10) << ".\n" ;
		m_result.array() /= m_nonmissingness.array().cast< double >() ;
	}

	void NormaliseGenotypesAndComputeXXtFast::compute_block(
		SampleBounds const& sample_bounds,
		std::vector< uint64_t > const& genotypes
	) {
		// only compute the lower diagonal.
		// We also want to make the function go down the rows of the result
		// in the inner loop, since it's stored column-major, so put row index in the inner loop.
		for( std::size_t j = sample_bounds.begin_sample_j; j < sample_bounds.end_sample_j; ++j ) {
			for( std::size_t i = std::max( j, std::size_t( sample_bounds.begin_sample_i ) ); i < sample_bounds.end_sample_i; ++i ) {
				uint64_t const combined_genotype = ( genotypes[i] << 2 ) + genotypes[j] ;
				std::size_t const lookup_index0 = combined_genotype & 0xFFFF ;
				std::size_t const lookup_index1 = ( combined_genotype >> 16 ) & 0xFFFF ;
				std::size_t const lookup_index2 = ( combined_genotype >> 32 ) & 0xFFFF ;
				std::size_t const lookup_index3 = ( combined_genotype >> 48 ) & 0xFFFF ;
				m_result(i,j) += m_lookup_tables[0][ lookup_index0 ] ;
				m_result(i,j) += m_lookup_tables[1][ lookup_index1 ] ;
				m_result(i,j) += m_lookup_tables[2][ lookup_index2 ] ;
				m_result(i,j) += m_lookup_tables[3][ lookup_index3 ] ;
				m_nonmissingness(i,j) += m_nonmissingness_lookup_tables[0][ lookup_index0 ] ;
				m_nonmissingness(i,j) += m_nonmissingness_lookup_tables[1][ lookup_index1 ] ;
				m_nonmissingness(i,j) += m_nonmissingness_lookup_tables[2][ lookup_index2 ] ;
				m_nonmissingness(i,j) += m_nonmissingness_lookup_tables[3][ lookup_index3 ] ;
			}
		}
	}
}

KinshipCoefficientComputer::KinshipCoefficientComputer(
	appcontext::OptionProcessor const& options,
	genfile::CohortIndividualSource const& samples,
	appcontext::UIContext& ui_context,
	Computation::UniquePtr computation
):
	m_options( options ),
	m_ui_context( ui_context ),
	m_samples( samples ),
	m_computation( computation )
{
	assert( m_computation.get() ) ;
}

void KinshipCoefficientComputer::begin_processing_snps( std::size_t number_of_samples, genfile::SNPDataSource::Metadata const& metadata ) {
	m_computation->begin_processing_snps( number_of_samples, metadata ) ;
}

void KinshipCoefficientComputer::processed_snp( genfile::VariantIdentifyingData const& id_data, genfile::VariantDataReader::SharedPtr data_reader ) {
	m_computation->processed_snp( id_data, data_reader ) ;
}

void KinshipCoefficientComputer::end_processing_snps() {
	m_computation->end_processing_snps() ;

	std::string description = "Number of SNPs: "
		+ genfile::string_utils::to_string( m_computation->number_of_snps_included() )
		+ "\nNumber of samples: "
		+ genfile::string_utils::to_string( m_samples.get_number_of_individuals() )
	;
#if 1
	send_results(
		m_computation->number_of_snps_included(),
		m_computation->nonmissingness().cast< double >(),
		m_computation->result().cast< double >(),
		"KinshipCoefficientComputer",
		description
	) ;
	#endif
}


#include <boost/test/unit_test.hpp>
#include <eosio/testing/tester.hpp>
#include <eosio/chain/global_property_object.hpp>
#include <boost/range/algorithm.hpp>

#ifdef NON_VALIDATING_TEST
#define TESTER tester
#else
#define TESTER validating_tester
#endif

using namespace eosio::testing;
using namespace eosio::chain;
using mvo = fc::mutable_variant_object;

BOOST_AUTO_TEST_SUITE(producer_schedule_tests)

   // Calculate expected producer given the schedule and slot number
   account_name get_expected_producer(const vector<producer_key>& schedule, const uint64_t slot) {
      const auto& index = (slot % (schedule.size() * config::producer_repetitions)) / config::producer_repetitions;
      return schedule.at(index).producer_name;
   };

   // Check if two schedule is equal
   bool is_schedule_equal(const vector<producer_key>& first, const vector<producer_key>& second) {
      bool is_equal = first.size() == second.size();
      for (uint32_t i = 0; i < first.size(); i++) {
         is_equal = is_equal && first.at(i) == second.at(i);
      }
      return is_equal;
   };

   // Calculate the block num of the next round first block
   // The new producer schedule will become effective when it's in the block of the next round first block
   // However, it won't be applied until the effective block num is deemed irreversible
   uint64_t calc_block_num_of_next_round_first_block(const controller& control){
      auto res = control.head_block_num() + 1;
      const auto blocks_per_round = control.head_block_state()->active_schedule.producers.size() * config::producer_repetitions;
      while((res % blocks_per_round) != 0) {
         res++;
      }
      return res;
   };
#if 0
   BOOST_FIXTURE_TEST_CASE( verify_producer_schedule, TESTER ) try {

      // Utility function to ensure that producer schedule work as expected
      const auto& confirm_schedule_correctness = [&](const vector<producer_key>& new_prod_schd, const uint64_t eff_new_prod_schd_block_num)  {
         const uint32_t check_duration = 1000; // number of blocks
         for (uint32_t i = 0; i < check_duration; ++i) {
            const auto current_schedule = control->head_block_state()->active_schedule.producers;
            const auto& current_absolute_slot = control->get_global_properties().proposed_schedule_block_num;
            // Determine expected producer
            const auto& expected_producer = get_expected_producer(current_schedule, *current_absolute_slot + 1);

            // The new schedule will only be applied once the effective block num is deemed irreversible
            const bool is_new_schedule_applied = control->last_irreversible_block_num() > eff_new_prod_schd_block_num;

            // Ensure that we have the correct schedule at the right time
            if (is_new_schedule_applied) {
               BOOST_TEST(is_schedule_equal(new_prod_schd, current_schedule));
            } else {
               BOOST_TEST(!is_schedule_equal(new_prod_schd, current_schedule));
            }

            // Produce block
            produce_block();

            // Check if the producer is the same as what we expect
            BOOST_TEST(control->head_block_producer() == expected_producer);
         }
      };

      // Create producer accounts
      vector<account_name> producers = {
              "inita", "initb", "initc", "initd", "inite", "initf", "initg",
              "inith", "initi", "initj", "initk", "initl", "initm", "initn",
              "inito", "initp", "initq", "initr", "inits", "initt", "initu"
      };
      create_accounts(producers);

      // ---- Test first set of producers ----
      // Send set prods action and confirm schedule correctness
      set_producers(producers);
      const auto first_prod_schd = get_producer_keys(producers);
      const auto eff_first_prod_schd_block_num = calc_block_num_of_next_round_first_block(*control);
      confirm_schedule_correctness(first_prod_schd, eff_first_prod_schd_block_num);

      // ---- Test second set of producers ----
      vector<account_name> second_set_of_producer = {
              producers[3], producers[6], producers[9], producers[12], producers[15], producers[18], producers[20]
      };
      // Send set prods action and confirm schedule correctness
      set_producers(second_set_of_producer);
      const auto second_prod_schd = get_producer_keys(second_set_of_producer);
      const auto& eff_second_prod_schd_block_num = calc_block_num_of_next_round_first_block(*control);
      confirm_schedule_correctness(second_prod_schd, eff_second_prod_schd_block_num);

      // ---- Test deliberately miss some blocks ----
      const int64_t num_of_missed_blocks = 5000;
      produce_block(fc::microseconds(500 * 1000 * num_of_missed_blocks));
      // Ensure schedule is still correct
      confirm_schedule_correctness(second_prod_schd, eff_second_prod_schd_block_num);
      produce_block();

      // ---- Test third set of producers ----
      vector<account_name> third_set_of_producer = {
              producers[2], producers[5], producers[8], producers[11], producers[14], producers[17], producers[20],
              producers[0], producers[3], producers[6], producers[9], producers[12], producers[15], producers[18],
              producers[1], producers[4], producers[7], producers[10], producers[13], producers[16], producers[19]
      };
      // Send set prods action and confirm schedule correctness
      set_producers(third_set_of_producer);
      const auto third_prod_schd = get_producer_keys(third_set_of_producer);
      const auto& eff_third_prod_schd_block_num = calc_block_num_of_next_round_first_block(*control);
      confirm_schedule_correctness(third_prod_schd, eff_third_prod_schd_block_num);

   } FC_LOG_AND_RETHROW()

   BOOST_FIXTURE_TEST_CASE( verify_producers, TESTER ) try {

      vector<account_name> valid_producers = {
         "inita", "initb", "initc", "initd", "inite", "initf", "initg",
         "inith", "initi", "initj", "initk", "initl", "initm", "initn",
         "inito", "initp", "initq", "initr", "inits", "initt", "initu"
      };
      create_accounts(valid_producers);
      set_producers(valid_producers);

      // account initz does not exist
      vector<account_name> nonexisting_producer = { "initz" };
      BOOST_CHECK_THROW(set_producers(nonexisting_producer), wasm_execution_error);

      // replace initg with inita, inita is now duplicate
      vector<account_name> invalid_producers = {
         "inita", "initb", "initc", "initd", "inite", "initf", "inita",
         "inith", "initi", "initj", "initk", "initl", "initm", "initn",
         "inito", "initp", "initq", "initr", "inits", "initt", "initu"
      };

      BOOST_CHECK_THROW(set_producers(invalid_producers), wasm_execution_error);

   } FC_LOG_AND_RETHROW()

   BOOST_FIXTURE_TEST_CASE( verify_header_schedule_version, TESTER ) try {

      // Utility function to ensure that producer schedule version in the header is correct
      const auto& confirm_header_schd_ver_correctness = [&](const uint64_t expected_version, const uint64_t eff_new_prod_schd_block_num)  {
         const uint32_t check_duration = 1000; // number of blocks
         for (uint32_t i = 0; i < check_duration; ++i) {
            // The new schedule will only be applied once the effective block num is deemed irreversible
            const bool is_new_schedule_applied = control->last_irreversible_block_num() > eff_new_prod_schd_block_num;

            // Produce block
            produce_block();

            // Ensure that the head block header is updated at the right time
            const auto current_schd_ver = control->head_block_header().schedule_version;
            if (is_new_schedule_applied) {
               BOOST_TEST(current_schd_ver == expected_version);
            } else {
               BOOST_TEST(current_schd_ver != expected_version);
            }
         }
      };

      // Create producer accounts
      vector<account_name> producers = {
              "inita", "initb", "initc", "initd", "inite", "initf", "initg",
              "inith", "initi", "initj", "initk", "initl", "initm", "initn",
              "inito", "initp", "initq", "initr", "inits", "initt", "initu"
      };
      create_accounts(producers);

      // Send set prods action and confirm schedule correctness
      set_producers(producers, 1);
      const auto& eff_first_prod_schd_block_num = calc_block_num_of_next_round_first_block(*control);
      // Confirm the version is correct
      confirm_header_schd_ver_correctness(1, eff_first_prod_schd_block_num);

      // Shuffle the producers and set the new one with smaller version
      boost::range::random_shuffle(producers);
      set_producers(producers, 0);
      const auto& eff_second_prod_schd_block_num = calc_block_num_of_next_round_first_block(*control);
      // Even though we set it with smaller version number, the version should be incremented instead
      confirm_header_schd_ver_correctness(2, eff_second_prod_schd_block_num);

      // Shuffle the producers and set the new one with much larger version number
      boost::range::random_shuffle(producers);
      set_producers(producers, 1000);
      const auto& eff_third_prod_schd_block_num = calc_block_num_of_next_round_first_block(*control);
      // Even though we set it with much large version number, the version should be incremented instead
      confirm_header_schd_ver_correctness(3, eff_third_prod_schd_block_num);

   } FC_LOG_AND_RETHROW()
#endif

BOOST_FIXTURE_TEST_CASE( producer_schedule_promotion_test, TESTER ) try {
   create_accounts( {N(alice),N(bob),N(carol)} );
   produce_block();

   auto compare_schedules = [&]( const vector<producer_key>& a, const producer_schedule_type& b ) {
      return std::equal( a.begin(), a.end(), b.producers.begin(), b.producers.end() );
   };

   auto res = set_producers( {N(alice),N(bob)} );
   vector<producer_key> sch1 = {
                                 {N(alice), get_public_key(N(alice), "active")},
                                 {N(bob),   get_public_key(N(bob),   "active")}
                               };
   //wdump((fc::json::to_pretty_string(res)));
   wlog("set producer schedule to [alice,bob]");
   BOOST_REQUIRE_EQUAL( true, control->proposed_producers().valid() );
   BOOST_CHECK_EQUAL( true, compare_schedules( sch1, *control->proposed_producers() ) );
   BOOST_CHECK_EQUAL( control->pending_producers().version, 0 );
   produce_block(); // Starts new block which promotes the proposed schedule to pending
   BOOST_CHECK_EQUAL( control->pending_producers().version, 1 );
   BOOST_CHECK_EQUAL( true, compare_schedules( sch1, control->pending_producers() ) );
   BOOST_CHECK_EQUAL( control->active_producers().version, 0 );
   produce_block();
   produce_block(); // Starts new block which promotes the pending schedule to active
   BOOST_CHECK_EQUAL( control->active_producers().version, 1 );
   BOOST_CHECK_EQUAL( true, compare_schedules( sch1, control->active_producers() ) );
   produce_blocks(7);

   res = set_producers( {N(alice),N(bob),N(carol)} );
   vector<producer_key> sch2 = {
                                 {N(alice), get_public_key(N(alice), "active")},
                                 {N(bob),   get_public_key(N(bob),   "active")},
                                 {N(carol), get_public_key(N(carol), "active")}
                               };
   wlog("set producer schedule to [alice,bob,carol]");
   BOOST_REQUIRE_EQUAL( true, control->proposed_producers().valid() );
   BOOST_CHECK_EQUAL( true, compare_schedules( sch2, *control->proposed_producers() ) );

   produce_block();
   produce_blocks(23); // Alice produces the last block of her first round.
                    // Bob's first block (which advances LIB to Alice's last block) is started but not finalized.
   BOOST_REQUIRE_EQUAL( control->head_block_producer(), N(alice) );
   BOOST_REQUIRE_EQUAL( control->pending_block_state()->header.producer, N(bob) );
   BOOST_CHECK_EQUAL( control->pending_producers().version, 2 );

   produce_blocks(12); // Bob produces his first 11 blocks
   BOOST_CHECK_EQUAL( control->active_producers().version, 1 );
   produce_blocks(12); // Bob produces his 12th block.
                    // Alice's first block of the second round is started but not finalized (which advances LIB to Bob's last block).
   BOOST_REQUIRE_EQUAL( control->head_block_producer(), N(alice) );
   BOOST_REQUIRE_EQUAL( control->pending_block_state()->header.producer, N(bob) );
   BOOST_CHECK_EQUAL( control->active_producers().version, 2 );
   BOOST_CHECK_EQUAL( true, compare_schedules( sch2, control->active_producers() ) );

   produce_block(); // Alice produces the first block of her second round which has changed the active schedule.

   // The next block will be produced according to the new schedule
   produce_block();
   BOOST_CHECK_EQUAL( control->head_block_producer(), N(carol) ); // And that next block happens to be produced by Carol.

   BOOST_REQUIRE_EQUAL( validate(), true );
} FC_LOG_AND_RETHROW()

BOOST_AUTO_TEST_SUITE_END()


/*
(c) 2015 Glen Joseph Fernandes
<glenjofe -at- gmail.com>

Distributed under the Boost Software
License, Version 1.0.
http://boost.org/LICENSE_1_0.txt
*/
#ifndef BOOST_ALIGN_ALIGN_DOWN_HPP
#define BOOST_ALIGN_ALIGN_DOWN_HPP

#include <boost/align/detail/align_down.hpp>
#include <boost/align/align_down_forward.hpp>

namespace boost {
namespace alignment {

BOOST_CONSTEXPR inline std::size_t align_down(std::size_t value,
    std::size_t alignment) BOOST_NOEXCEPT
{
    return value & ~(alignment - 1);
}

} /* .alignment */
} /* .boost */

#endif


/*******************************************************************************
 * Copyright (c) 2012, Dougal J. Sutherland (dsutherl@cs.cmu.edu).             *
 * All rights reserved.                                                        *
 *                                                                             *
 * Redistribution and use in source and binary forms, with or without          *
 * modification, are permitted provided that the following conditions are met: *
 *                                                                             *
 *     * Redistributions of source code must retain the above copyright        *
 *       notice, this list of conditions and the following disclaimer.         *
 *                                                                             *
 *     * Redistributions in binary form must reproduce the above copyright     *
 *       notice, this list of conditions and the following disclaimer in the   *
 *       documentation and/or other materials provided with the distribution.  *
 *                                                                             *
 *     * Neither the name of Carnegie Mellon University nor the                *
 *       names of the contributors may be used to endorse or promote products  *
 *       derived from this software without specific prior written permission. *
 *                                                                             *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE   *
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR         *
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF        *
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS    *
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN     *
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)     *
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE  *
 * POSSIBILITY OF SUCH DAMAGE.                                                 *
 ******************************************************************************/
#include "sdm/basics.hpp"
#include "sdm/sdm.hpp"
#include "sdm/kernels/linear.hpp"
#include "sdm/kernels/polynomial.hpp"
#include "sdm/kernels/gaussian.hpp"
#include "sdm/kernels/from_str.hpp"

#include <algorithm>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

#include <boost/bind.hpp>
#include <boost/format.hpp>
#include <boost/program_options.hpp>
#include <boost/throw_exception.hpp>
#include <boost/utility.hpp>

#include <flann/flann.hpp>

#include <np-divs/matrix_arrays.hpp>
#include <np-divs/matrix_io.hpp>
#include <np-divs/div-funcs/from_str.hpp>
#include <np-divs/div_params.hpp>

// TODO: support regression
// TODO: support CV
// TODO: warn about dumb parameter combos, like linear kernel with distance df

using std::cerr;
using std::cin;
using std::cout;
using std::endl;
using std::ifstream;
using std::string;
using std::vector;

namespace po = boost::program_options;

// TODO: leaks memory everywhere, but whatever...
struct ProgOpts : boost::noncopyable {
    string train_bags_file;
    string test_bags_file;
    string cv_divs_file;

    npdivs::DivFunc * div_func;
    sdm::KernelGroup * kernel_group;

    size_t k;
    size_t num_threads;
    size_t cv_folds;
    size_t cv_threads;
    size_t tuning_folds;
    bool prob;
    bool proj_indiv;
    bool shuffle;

    flann::IndexParams index_params;
    flann::SearchParams search_params;

    void parse_div_func(const string &spec) {
        div_func = npdivs::div_func_from_str(spec);
    }

    void parse_kernel(const string &name) {
        kernel_group = sdm::kernel_group_from_str(name);
    }

    void parse_index(const string &name) {
        index_params = npdivs::index_params_from_str(name);
    }
};

bool parse_args(int argc, char ** argv, ProgOpts& opts);

int main(int argc, char ** argv) {
    typedef flann::Matrix<double> Matrix;

    try {
        ProgOpts opts;
        if (!parse_args(argc, argv, opts))
            return 1;

        // TODO: configurable logging
        FILELog::ReportingLevel() = logDEBUG2;
        // Output2FILE::Stream() = stderr;

        // TODO: gracefully handle nonexisting files
        // TODO: more robust input checking

        // load training bags
        size_t num_train;
        Matrix* train_bags;
        vector<string> train_labels;

        if (opts.train_bags_file == "-") {
            cout << "Enter training distributions in CSV-like "
                "format: an initial string label for each distribution, one "
                "line with comma-separated floating-point values for each "
                "point, one blank line between distributions, and an extra "
                "blank line when done.\n";
            train_bags = npdivs::labeled_matrices_from_csv(
                    std::cin, num_train, train_labels);
        } else {
            FILE_LOG(logINFO) << "Loading training bags...";
            ifstream ifs(opts.train_bags_file.c_str(), ifstream::in);
            train_bags = npdivs::labeled_matrices_from_csv(
                    ifs, num_train, train_labels);
            FILE_LOG(logINFO) << "done.";
        }

        // load divergences, maybe
        bool do_cv = !opts.cv_divs_file.empty() || opts.test_bags_file.empty();

        double* cv_divs = NULL;
        if (!opts.cv_divs_file.empty()) {
            flann::Matrix<double> cv_divs_m;
            if (opts.cv_divs_file == "-") {
                cout << "Enter the matrix of divergences among training bags "
                    "in CSV-like format.";
                cv_divs_m = npdivs::matrix_from_csv(cin);
            } else {
                FILE_LOG(logINFO) << "Loading divergences...";
                ifstream ifs(opts.cv_divs_file.c_str(), ifstream::in);
                cv_divs_m = npdivs::matrix_from_csv(ifs);
                FILE_LOG(logINFO) << "done.";
            }

            if (cv_divs_m.rows != num_train || cv_divs_m.cols != num_train) {
                FILE_LOG(logERROR) << boost::format(
                        "Input divergences are %dx%d, expected %dx%d. "
                        "Ignoring them.")
                    % cv_divs_m.rows % cv_divs_m.cols
                    % num_train % num_train;
                do_cv = opts.test_bags_file.empty();
            } else {
                cv_divs = cv_divs_m.ptr();
            }
        }

        // load test bags, maybe
        size_t num_test = 0;
        Matrix* test_bags;
        vector<string> test_labels;

        if (!do_cv) {
            if (opts.test_bags_file == "-") {
                cout << "Enter testing distributions in CSV-like "
                    "format: an initial string label for each distribution, "
                    "one line with comma-separated floating-point values for "
                    "each point, one blank line between distributions, and an "
                    "extra blank line when done.\n";
                test_bags = npdivs::labeled_matrices_from_csv(
                        cin, num_test, test_labels);
            } else {
                FILE_LOG(logINFO) << "Loading test bags...";
                ifstream ifs(opts.test_bags_file.c_str(), ifstream::in);
                test_bags = npdivs::labeled_matrices_from_csv(
                        ifs, num_test, test_labels);
                FILE_LOG(logINFO) << "done.";
            }
        }

        // convert labels into integers
        // make maps to convert between int / string labels
        int this_label = -1;
        std::map<string,int> labels_to_int;
        std::map<int,string> labels_to_string;

        labels_to_int[""] = -1;
        labels_to_int["?"] = -1;
        labels_to_string[-1] = "?";

        std::vector<int> train_labels_ints;
        train_labels_ints.reserve(num_train);
        for (size_t i = 0; i < num_train; i++) {
            const string &lbl = train_labels[i];

            if (labels_to_int.count(lbl) == 0) {
                labels_to_int[lbl] = ++this_label;
                labels_to_string[this_label] = lbl;
            }
            train_labels_ints.push_back(labels_to_int[lbl]);
        }

        std::vector<int> test_labels_ints;
        test_labels_ints.reserve(num_test);
        for (size_t i = 0; i < num_test; i++) {
            const string &lbl = test_labels[i];

            if (labels_to_int.count(lbl) == 0) {
                labels_to_int[lbl] = ++this_label;
                labels_to_string[this_label] = lbl;
            }
            test_labels_ints.push_back(labels_to_int[lbl]);
        }

        // div params
        npdivs::DivParams div_params(opts.k,
                opts.index_params, opts.search_params,
                opts.num_threads);

        // svm params
        svm_parameter svm_params(sdm::default_svm_params);
        svm_params.probability = (int) opts.prob;

        if (!do_cv) {
            // train the model
            sdm::SDM<double, int>* model = train_sdm(
                    train_bags, num_train, train_labels_ints,
                    *opts.div_func, *opts.kernel_group, div_params,
                    sdm::default_c_vals, svm_params, opts.tuning_folds);

            cout << "Trained model: " << model->name() << endl;

            // predict on test data
            const std::vector<int> &preds = model->predict(test_bags, num_test);

            // output predictions // TODO: optionally into file?
            // tally up accuracy at the same time
            size_t num_correct = 0;
            size_t total = num_test;

            cout << "Predicted labels:\n";
            for (size_t i = 0; i < num_test; i++) {
                cout << i << ":\t" << labels_to_string[preds[i]];
                if (test_labels_ints[i] == -1) {
                    total--;
                } else {
                    if (test_labels_ints[i] == preds[i]) {
                        cout << "\t -- correct";
                        num_correct++;
                    } else {
                        cout << "\t -- expected " << test_labels[i];
                    }
                }
                cout << endl;
            }
            cout << endl;

            if (total > 0) {
                cout << "Accuracy on " << total << " labeled test points: "
                    << num_correct * 100. / total << "%\n";
            }

            // cleanup
            model->destroyModelAndProb();
            delete model;
        } else {
            double acc = crossvalidate(train_bags, num_train,
                    train_labels_ints, *opts.div_func, *opts.kernel_group,
                    div_params, opts.cv_folds, opts.cv_threads,
                    !opts.proj_indiv, opts.shuffle,
                    sdm::default_c_vals, svm_params,
                    opts.tuning_folds, cv_divs);
            cout << opts.cv_folds << "-fold cross-validation accuracy on "
                << num_train << " bags: " << 100. * acc << "%" << endl;
        }

        // cleanup
        npdivs::free_matrix_array(train_bags, num_train);
        if (!do_cv)
            npdivs::free_matrix_array(test_bags, num_test);

    } catch (std::exception &e) {
        cerr << "Error: " << e.what() << endl;
        return 1;
    }
}

bool parse_args(int argc, char ** argv, ProgOpts& opts) {
    po::options_description desc("Allowed options");
    desc.add_options()
        ("help,h", "Produce this help message.")
        ("train-bags,b",
            po::value<string>(&opts.train_bags_file)->default_value("-"),
            "CSV-like file containing matrices separated by blank lines, "
            "with a string label on its own line before each matrix; "
            "- means stdin.")
        ("test-bags,t",
            po::value<string>(&opts.test_bags_file)->default_value(""),
            "CSV-style file containing matrices separated by blank lines, "
            "with a string label on its own line before each matrix."
            "- means stdin. "
            "\nUse a blank line or ? as the label if it's not known. If any "
            "test distributions are labeled, will report accuracy on them.")
        ("cv-divs,D",
            po::value<string>(&opts.cv_divs_file)->default_value(""),
            "CSV file containing precomputed divergences among all the bags "
            "to use for cross-validation. Still reads --train-bags for labels "
            "only; --test-bags is ignored.")
        // TODO: support giving labels only
        // TODO: test divs support
        ("cv-folds,c",
            po::value<size_t>(&opts.cv_folds)->default_value(10),
            "Do c-fold cross-validation on the data passed in --train-bags. "
            "Ignored if --test-bags is passed.")
        ("cv-threads",
            po::value<size_t>(&opts.cv_threads)->default_value(0),
            "The max number of CV folds to do in parallel; defaults to the "
            "value of --num-threads. Will not use more than "
            "max(cv_threads, num_threads) threads at any point.")
        ("div-func,d",
            po::value<string>()->default_value("l2")->notifier(
                boost::bind(&ProgOpts::parse_div_func, boost::ref(opts), _1)),
            "Divergence function to use. Format is name[:arg1[:arg2...]]. "
            "Options include alpha, bc, hellinger, l2, linear, renyi. "
            "\nalpha and renyi take an optional second argument, so that "
            "e.g. renyi:.8 is the Renyi-.8 divergence."
            "\nAll options take a last argument that specifies how large "
            " intermediate values are  normalized; the default .99 means to "
            "cap certain values at the 99th percentile of their distribution, "
            "and 1 means not to do this.")
        ("kernel,k",
            po::value<string>()->default_value("gaussian")->notifier(
                boost::bind(&ProgOpts::parse_kernel, boost::ref(opts), _1)),
            "Kernel type to use. Options are gaussian, linear, polynomial. "
            "\nUse '-k linear' or '-k polynomial' only with '-d linear'; any "
            "of the other -d arguments should be used with '-k gaussian' "
            "in order to get a meaningful kernel. "
            "\nNote that cross-validation is done to select gaussian kernel "
            "width or polynomial degree, in combination with SVM C; this is "
            "not yet configurable through this interface.")
        ("tuning-folds,f",
            po::value<size_t>(&opts.tuning_folds)->default_value(3),
            "The number of folds to use for the parameter tuning "
            "cross-validation.")
        ("probability,P",
            po::value<bool>(&opts.prob)->default_value(0)->zero_tokens(),
            "Use probability estimates in the trained SVMs.")
        ("project-individually,J",
            po::value<bool>(&opts.proj_indiv)->default_value(0)->zero_tokens(),
            "When cross-validating, do the PSD projection on each fold's "
            "training data only, rather than on the entire kernel matrix.")
        ("shuffle",
            po::value<bool>(&opts.shuffle)->default_value(1),
            "Whether to shuffle the order of folds in cross-validation.")
        ("num-threads,T",
            po::value<size_t>(&opts.num_threads)->default_value(0),
            "Number of threads to use for calculations. 0 means one per core "
            "if compiled with recent-enough boost, or one thread otherwise.")
        ("neighbors,K",
            po::value<size_t>(&opts.k)->default_value(3),
            "The k for k-nearest-neighbor calculations.")
        ("index,i",
            po::value<string>()->default_value("kdtree")->notifier(
                boost::bind(&ProgOpts::parse_index, boost::ref(opts), _1)),
            "The nearest-neighbor index to use. Options: linear, kdtree. "
            "\nNote that this can have a large effect on calculation time: "
            "use kdtree for low-dimensional data and linear for relatively "
            "sparse high-dimensional data (about about 10).")
    ;

    po::variables_map vm;
    try {
        po::store(po::command_line_parser(argc, argv).options(desc).run(), vm);

        if (vm.count("help")) {
            cout << desc << endl;
            std::exit(0);
        }

        po::notify(vm);

    } catch (std::exception &e) {
        cerr << "Error: " << e.what() << endl;
        return false;
    }

    opts.search_params = flann::SearchParams(64);

    return true;
}


/**----------------------------------------------------------------------------
 Copyright  2018-, UT-Battelle, LLC
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:

 * Redistributions of source code must retain the above copyright notice, this
 list of conditions and the following disclaimer.

 * Redistributions in binary form must reproduce the above copyright notice,
 this list of conditions and the following disclaimer in the documentation
 and/or other materials provided with the distribution.

 * Neither the name of the copyright holder nor the names of its
 contributors may be used to endorse or promote products derived from
 this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 Author(s): Jay Jay Billings (billingsjj <at> ornl <dot> gov)
 -----------------------------------------------------------------------------*/
#define BOOST_TEST_DYN_LINK
#define BOOST_TEST_MODULE kelvin

#include <ConstitutiveRelationshipService.h>
#include <ConstitutiveRelationship.h>
#include <Grid.h>
#include <MaterialPoint.h>
#include <boost/test/included/unit_test.hpp>
#include <memory>
#include <mfem.hpp>
#include <vector>
#include <MeshContainer.h>
#include <H1FESpaceFactory.h>
#include <INIPropertyParser.h>

using namespace std;
using namespace mfem;
using namespace Kelvin;
using namespace fire;

class TestConstitutiveRelationship : public ConstitutiveRelationship {
public:

	virtual void updateStrainRate(const Kelvin::Grid & grid,
			Kelvin::MaterialPoint & matPoint) {

		// Just set the strains to 5.0.
		int dim = matPoint.dimension();
		for (int j = 0; j < dim; j++) {
			for (int k = 0; k < dim; k++) {
				matPoint.strain[j][k] = 5.0;
			}
		}

	}

	virtual void updateStress(const Kelvin::Grid & grid,
			Kelvin::MaterialPoint & matPoints) {

	}

	virtual ~TestConstitutiveRelationship() {};
};

// Test file names
static std::string inputFile = "2SquaresInput-smallerMesh.ini";

/**
 * This operation checks the basic functionality of the constitutive
 * relationship service.
 */
BOOST_AUTO_TEST_CASE(checkServiceRegistration) {

	// Create the test pointer
	unique_ptr<ConstitutiveRelationship> testRelPtr =
			make_unique<TestConstitutiveRelationship>();
	// Add it to the service
	ConstitutiveRelationshipService::add(8675309,std::move(testRelPtr));

	// Create the space factory
	H1FESpaceFactory spaceFactory;

	// Load the input file
	INIPropertyParser propertyParser;
	propertyParser.setSource(inputFile);
    propertyParser.parse();

	// Load the mesh
    MeshContainer mc(propertyParser.getPropertyBlock("mesh"),spaceFactory);

    // Create the grid
    Grid grid(mc);
    // Just use the quadrature points as the particles
    auto points = mc.getQuadraturePoints();
    std::vector<MaterialPoint> mPoints;
    for (int i = 0; i < points.size(); i++) {
    	MaterialPoint point(points[i]);
    	mPoints.push_back(point);
    }

	// Get the test relationship
	auto & relationship = ConstitutiveRelationshipService::get(8675309);

	// Check them
	int dim = mPoints[0].dimension();
	for (int i = 0; i < mPoints.size(); i++) {
		// Update the strain using the relationship
		relationship.updateStrainRate(grid,mPoints[i]);
		auto & point = mPoints[i];
		for (int j = 0; j < dim; j++) {
			for (int k = 0; k < dim; k++) {
				BOOST_REQUIRE_CLOSE(5.0,point.strain[j][k],1.0e-5);
			}
		}
	}

	return;
}


//
//  KDTree.hpp
//
//
//  Created by Sivaram Ambikasaran on 12/3/13.
//
//

#ifndef __KDTREE_HPP__
#define __KDTREE_HPP__

#include <iostream>
#include <Eigen/Dense>

class KDTree {
private:
        unsigned n_Locations;                   //      Number of locations.
        unsigned n_Dimension;                   //      Number of dimensions.
        unsigned n_Properties;                  //      Number of properties.
        unsigned MinParticlesInLeaf;
        unsigned nLevels;

        Eigen::MatrixXd sorted_Contents;        //      Array of sorted contents, that contains both the locations and properties.

        //      Merges two sorted lists.
        //      Usage:
        //      merge_Sorted_Lists(7, 20, 9, 50, 0);
        void merge_Sorted_Lists(unsigned n_Left_Start, unsigned n_Left_Size, unsigned n_Right_Start, unsigned n_Right_Size, unsigned n_Index);

        //      Performs a merge sort on a list given the starting index, size of the set and index based on which the sorting needs to be done.
        //      Usage:
        //      merge_Sort(11, 5, 1);
        void merge_Sort(unsigned n_Start, unsigned n_Size, unsigned n_Index);

        //      Given the starting index, size of the set of points and the index (which has to be less than n_Dimension) based on which to sort, this function performs a sort based on KDTree.
        //      Usage:
        //      sort_KDTree(7, 193, 2);
        int sort_KDTree(unsigned n_Start, unsigned n_Size, unsigned n_Index, unsigned local_level, std::vector<int>& NumberOfParticlesInLeaves);

public:
        //      Constructor.
        //      Usage:
        //      KDTree(10000, 3, locations);
        KDTree(const unsigned n_Locations, const unsigned n_Dimension, double* locations, const unsigned n_Properties, double* properties, const unsigned MinParticlesInLeaf, const unsigned nLevels);

        //      KDTree sorting.
        int sort_KDTree(std::vector<int>& NumberOfParticlesInLeaves);

        //      Obtains all the sorted locations and properties.
        //      Usage:
        //      get_Location(location, properties);
        //      Obtains all the locations and properties in sorted fashion.
        void get_Location_Properties(double* locations, double* properties);

        //      Given an index, obtains the location and property from the sorted location and property.
        //      n_Index <= n_Locations.
        //      Usage:
        //      get_Location_Properties(1234, location, properties);
        //      Obtains the location and properties corresponding to the 1234 index in the sorted fashion.
        void get_Location_Properties(const unsigned n_Index, double* location, double* properties);
};
#endif /* defined(__KDTREE_HPP__) */


#include <boost/accumulators/statistics.hpp>


#ifndef STAN_MATH_PRIM_PROB_POISSON_RNG_HPP
#define STAN_MATH_PRIM_PROB_POISSON_RNG_HPP

#include <stan/math/prim/meta.hpp>
#include <stan/math/prim/err.hpp>
#include <stan/math/prim/fun/constants.hpp>
#include <stan/math/prim/fun/size.hpp>
#include <boost/random/poisson_distribution.hpp>
#include <boost/random/variate_generator.hpp>

namespace stan {
namespace math {

/** \ingroup prob_dists
 * Return a Poisson random variate with specified rate parameter
 * using the given random number generator.
 *
 * lambda can be a scalar or a one-dimensional container.
 *
 * @tparam T_rate type of rate parameter
 * @tparam RNG type of random number generator
 * @param lambda (Sequence of) rate parameter(s)
 * @param rng random number generator
 * @return (Sequence of) Poisson random variate(s)
 * @throw std::domain_error if lambda is nonpositive
 */
template <typename T_rate, class RNG>
inline typename VectorBuilder<true, int, T_rate>::type poisson_rng(
    const T_rate& lambda, RNG& rng) {
  using boost::random::poisson_distribution;
  using boost::variate_generator;

  static const char* function = "poisson_rng";

  check_not_nan(function, "Rate parameter", lambda);
  check_positive(function, "Rate parameter", lambda);
  check_less(function, "Rate parameter", lambda, POISSON_MAX_RATE);

  scalar_seq_view<T_rate> lambda_vec(lambda);
  size_t N = size(lambda);
  VectorBuilder<true, int, T_rate> output(N);

  for (size_t n = 0; n < N; ++n) {
    variate_generator<RNG&, poisson_distribution<> > poisson_rng(
        rng, poisson_distribution<>(lambda_vec[n]));
    output[n] = poisson_rng();
  }

  return output.data();
}

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


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

// Copyright (c) 2007-2012 Barend Gehrels, Amsterdam, the Netherlands.
// Copyright (c) 2008-2012 Bruno Lalande, Paris, France.
// Copyright (c) 2009-2012 Mateusz Loskot, London, UK.

// Parts of Boost.Geometry are redesigned from Geodan's Geographic Library
// (geolib/GGL), copyright (c) 1995-2010 Geodan, Amsterdam, the Netherlands.

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

#ifndef BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MAP_TRANSFORMER_HPP
#define BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MAP_TRANSFORMER_HPP

#include <cstddef>

#include <boost/geometry/strategies/transform/matrix_transformers.hpp>

namespace boost { namespace geometry
{

// Silence warning C4127: conditional expression is constant
#if defined(_MSC_VER)
#pragma warning(push)
#pragma warning(disable : 4127)
#endif

namespace strategy { namespace transform
{

/*!
\brief Transformation strategy to map from one to another Cartesian coordinate system
\ingroup strategies
\tparam Mirror if true map is mirrored upside-down (in most cases pixels
    are from top to bottom, while map is from bottom to top)
 */
template
<
    typename CalculationType,
    std::size_t Dimension1,
    std::size_t Dimension2,
    bool Mirror = false,
    bool SameScale = true
>
class map_transformer
    : public ublas_transformer<CalculationType, Dimension1, Dimension2>
{
    typedef boost::numeric::ublas::matrix<CalculationType> M;

public :
    template <typename B, typename D>
    explicit inline map_transformer(B const& box, D const& width, D const& height)
    {
        set_transformation(
                get<min_corner, 0>(box), get<min_corner, 1>(box),
                get<max_corner, 0>(box), get<max_corner, 1>(box),
                width, height);
    }

    template <typename W, typename D>
    explicit inline map_transformer(W const& wx1, W const& wy1, W const& wx2, W const& wy2,
                        D const& width, D const& height)
    {
        set_transformation(wx1, wy1, wx2, wy2, width, height);
    }

private :
    template <typename W, typename P, typename S>
    inline void set_transformation_point(W const& wx, W const& wy,
        P const& px, P const& py,
        S const& scalex, S const& scaley)
    {

        // Translate to a coordinate system centered on world coordinates (-wx, -wy)
        M t1(3,3);
        t1(0,0) = 1;   t1(0,1) = 0;   t1(0,2) = -wx;
        t1(1,0) = 0;   t1(1,1) = 1;   t1(1,2) = -wy;
        t1(2,0) = 0;   t1(2,1) = 0;   t1(2,2) = 1;

        // Scale the map
        M s(3,3);
        s(0,0) = scalex;   s(0,1) = 0;   s(0,2) = 0;
        s(1,0) = 0;    s(1,1) = scaley;  s(1,2) = 0;
        s(2,0) = 0;    s(2,1) = 0;      s(2,2) = 1;

        // Translate to a coordinate system centered on the specified pixels (+px, +py)
        M t2(3, 3);
        t2(0,0) = 1;   t2(0,1) = 0;   t2(0,2) = px;
        t2(1,0) = 0;   t2(1,1) = 1;   t2(1,2) = py;
        t2(2,0) = 0;   t2(2,1) = 0;   t2(2,2) = 1;

        // Calculate combination matrix in two steps
        this->m_matrix = boost::numeric::ublas::prod(s, t1);
        this->m_matrix = boost::numeric::ublas::prod(t2, this->m_matrix);
    }

    template <typename W, typename D>
    void set_transformation(W const& wx1, W const& wy1, W const& wx2, W const& wy2,
                    D const& width, D const& height)
    {
        D px1 = 0;
        D py1 = 0;
        D px2 = width;
        D py2 = height;

        // Get the same type, but at least a double
        typedef typename select_most_precise<D, double>::type type;

        // Calculate appropriate scale, take min because whole box must fit
        // Scale is in PIXELS/MAPUNITS (meters)
        W wdx = wx2 - wx1;
        W wdy = wy2 - wy1;
        type sx = (px2 - px1) / boost::numeric_cast<type>(wdx);
        type sy = (py2 - py1) / boost::numeric_cast<type>(wdy);

        if (SameScale)
        {
            type scale = (std::min)(sx, sy);
            sx = scale;
            sy = scale;
        }

        // Calculate centerpoints
        W wtx = wx1 + wx2;
        W wty = wy1 + wy2;
        W two = 2;
        W wmx = wtx / two;
        W wmy = wty / two;
        type pmx = (px1 + px2) / 2.0;
        type pmy = (py1 + py2) / 2.0;

        set_transformation_point(wmx, wmy, pmx, pmy, sx, sy);

        if (Mirror)
        {
            // Mirror in y-direction
            M m(3,3);
            m(0,0) = 1;   m(0,1) = 0;   m(0,2) = 0;
            m(1,0) = 0;   m(1,1) = -1;  m(1,2) = 0;
            m(2,0) = 0;   m(2,1) = 0;   m(2,2) = 1;

            // Translate in y-direction such that it fits again
            M y(3, 3);
            y(0,0) = 1;   y(0,1) = 0;   y(0,2) = 0;
            y(1,0) = 0;   y(1,1) = 1;   y(1,2) = height;
            y(2,0) = 0;   y(2,1) = 0;   y(2,2) = 1;

            // Calculate combination matrix in two steps
            this->m_matrix = boost::numeric::ublas::prod(m, this->m_matrix);
            this->m_matrix = boost::numeric::ublas::prod(y, this->m_matrix);
        }
    }
};

}} // namespace strategy::transform

#if defined(_MSC_VER)
#pragma warning(pop)
#endif

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_TRANSFORM_MAP_TRANSFORMER_HPP


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

// Copyright (c) 2011-2015 Barend Gehrels, Amsterdam, the Netherlands.

// This file was modified by Oracle on 2021.
// Modifications copyright (c) 2021, Oracle and/or its affiliates.
// Contributed and/or modified by Adam Wulkiewicz, on behalf of Oracle

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

#include <geometry_test_common.hpp>

#include <boost/range/empty.hpp>

#include <boost/geometry/algorithms/clear.hpp>
#include <boost/geometry/algorithms/num_points.hpp>

#include <boost/geometry/io/wkt/wkt.hpp>

#include <boost/geometry/geometries/adapted/boost_variant.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/geometry_collection.hpp>
#include <boost/geometry/geometries/point_xy.hpp>

template <typename Geometry>
void test_geometry(std::string const& wkt, std::size_t expected_before, std::size_t expected_after)
{
    Geometry geometry;
    bg::read_wkt(wkt, geometry);
    boost::variant<Geometry> variant_geometry(geometry);
    bg::model::geometry_collection<boost::variant<Geometry>> gc{ variant_geometry };

    BOOST_CHECK_EQUAL(bg::num_points(geometry), expected_before);
    bg::clear(geometry);
    BOOST_CHECK_EQUAL(bg::num_points(geometry), expected_after);

    BOOST_CHECK_EQUAL(bg::num_points(variant_geometry), expected_before);
    bg::clear(variant_geometry);
    BOOST_CHECK_EQUAL(bg::num_points(variant_geometry), expected_after);

    BOOST_CHECK(! boost::empty(gc));
    bg::clear(gc);
    BOOST_CHECK(boost::empty(gc));
}

template <typename Point>
void test_all()
{
    typedef bg::model::polygon<Point> poly;
    typedef bg::model::linestring<Point> ls;
    typedef bg::model::multi_point<Point> mpoint;
    typedef bg::model::multi_linestring<ls> mls;
    typedef bg::model::multi_polygon<poly> mpoly;

    test_geometry<Point>("POINT(0 0)", 1, 1);
    test_geometry<ls>("LINESTRING(0 0,0 1)", 2, 0);
    test_geometry<poly>("POLYGON((0 0,0 1,1 0,0 0))", 4, 0);
    test_geometry<mpoint>("MULTIPOINT((0 0),(0 1),(1 0),(0 0))", 4, 0);
    test_geometry<mls>("MULTILINESTRING((0 0,0 1),(1 0,0 0))", 4, 0);
    test_geometry<mpoly>("MULTIPOLYGON(((0 0,0 1,1 0,0 0)),((10 0,10 1,11 0,10 0)))", 8, 0);
}

int test_main( int , char* [] )
{
    test_all<bg::model::d2::point_xy<double> >();

    return 0;
}


/*
 *  Copyright (C) 2018 Naomasa Matsubayashi
 *  Licensed under MIT license, see file LICENSE in this source tree.
 */
#ifndef UWG_COOKIE_HPP
#define UWG_COOKIE_HPP
#include <iterator>
#include <boost/spirit/include/karma.hpp>
#include <boost/spirit/include/qi.hpp>
#include <sodium/crypto_aead_xchacha20poly1305.h>
#include <uwg/hash.hpp>
#include <uwg/mac.hpp>
#include <uwg/kdf.hpp>
#include <uwg/dh.hpp>
#include <uwg/xaead.hpp>
#include <uwg/timestamp.hpp>
#include <uwg/initial_key.hpp>
#include <uwg/clear_key.hpp>
#include <uwg/types.hpp>
#include <uwg/dump.hpp>
namespace uwg {
  template< typename Out >
  void generate_cookie_key( Out &out ) {
    out.resize( wg_key_len );
    randombytes_buf( out.data(), out.size() );
  }
  template< typename In >
  void parse_cookie( wg_key_type &cookie, const wg_key_type &self_static_public, const wg_key_type &mac1, const In &in ) {
    if( in.size() != wg_cookie_len ) throw invalid_packet();
    if( in[ 0 ] != 0x03 ) throw invalid_packet();
    constexpr static char label_mac1[] = "cookie--";
    wg_key_type aead_key;
    hash().update( label_mac1, label_mac1 + strlen( label_mac1 ) ).update( self_static_public ).get( aead_key );
    auto incoming_nonce = make_svv( in.data(), wg_cookie_nonce_offset, wg_nonce_len );
    auto encrypted_cookie = make_svv( in.data(), wg_cookie_cookie_offset, wg_cookie_len );
    if( !xaead_dec( cookie, aead_key, incoming_nonce, encrypted_cookie, mac1 ) )
      throw invalid_packet();
  }

}

#endif