/*~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~\
|  Phycas: Python software for phylogenetic analysis                          |
|  Copyright (C) 2006 Mark T. Holder, Paul O. Lewis and David L. Swofford     |
|                                                                             |
|  This program is free software; you can redistribute it and/or modify       |
|  it under the terms of the GNU General Public License as published by       |
|  the Free Software Foundation; either version 2 of the License, or          |
|  (at your option) any later version.                                        |
|                                                                             |
|  This program is distributed in the hope that it will be useful,            |
|  but WITHOUT ANY WARRANTY; without even the implied warranty of             |
|  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the              |
|  GNU General Public License for more details.                               |
|                                                                             |
|  You should have received a copy of the GNU General Public License along    |
|  with this program; if not, write to the Free Software Foundation, Inc.,    |
|  51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.                |
\~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/

#include <fstream>
#include <strstream>
#include <boost/lambda/lambda.hpp>
#include <boost/lambda/bind.hpp>
#include <boost/format.hpp>
#include <boost/foreach.hpp>
#include <boost/algorithm/string/join.hpp>
#include "ncl/nxsallocatematrix.h"
#include "basic_lot.hpp"
#include "basic_lot.hpp"
#include "varcov.hpp"
#include "xlikelihood.hpp"
#include <boost/math/special_functions/gamma.hpp>   // lgamma
using namespace phycas;

/*----------------------------------------------------------------------------------------------------------------------
| 	Allocates a two dimensional array of doubles as one contiguous block of memory
| 	the dimensions are f by s.
|
| 	The pointer should be freed by a call to DeleteTwoDArrayDouble
|
| 	The array is set up so that:
|
|	for (i = 0 ; i < f ; i++)
|		for (j = 0 ; j < s ; j++)
|			array[i][j];
|
|	would be the same order of access as:
|
|  	double * ptr = **array;
|	for (i = 0 ; i < f*s*t ; i++)
|		*ptr++;
|
|   This is a template specialization for the NewTwoDArray function defined in nxsallocatematrix.h.
*/
double * * NewTwoDArrayDouble(unsigned f, unsigned s)
	{
	assert(f > 0 && s > 0);
	double * * ptr;
	ptr = new double *[f];
	*ptr = new double [f * s];
	for (unsigned fIt = 1 ; fIt < f ; fIt ++)
		ptr[fIt] = ptr[fIt -1] +  s;
	return ptr;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Deletes a 2-dimensional array of doubles and sets ptr to NULL.
|
|   This is a template specialization for the DeleteTwoDArray function defined in nxsallocatematrix.h.
*/
void DeleteTwoDArrayDouble(double * *  & ptr)
	{
	if (ptr)
		{
		delete [] * ptr;
		delete [] ptr;
		ptr = NULL;
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Allocates memory for all vectors and matrices: _V, _S, _Sinv, and _z are all p by p; _posterior_samples has length
|   n (but each element has a _parameters vector of length p), and _w is of length p.
*/
VarCovMatrix::VarCovMatrix(std::string name, unsigned n, unsigned p)
  : _n(0), _p(0), _V(0), _S(0), _Stmp(0), _Sinv(0), _z(0), _shell_midpoint(0.0), _shell_delta(0.0)
    , _log_ratio_sum(0.0), _representative_logkernel(0.0), _representative_loglikelihood(0.0), _representative_logprior(0.0)
    , _representative_logjacobian_edgelen(0.0), _representative_logjacobian_substmodel(0.0), _representative_logjacobian_standardization(0.0)
    , _center_around_mode(false) // currently _center_around_mode cannot be modified anywhere except here
	{
	PHYCAS_ASSERT(n > 0);
	PHYCAS_ASSERT(p > 0);
	_n = n;
	_p = p;
    _name = name;

	// Create _V (variance-covariance matrix)
	_V = NewTwoDArrayDouble(_p, _p);

	// Create _S (square root of variance-covariance matrix)
	_S = NewTwoDArrayDouble(_p, _p);

	// Create _Sinv (inverse square root of variance-covariance matrix)
	_Sinv = NewTwoDArrayDouble(_p, _p);

	// Create _Stmp (inverse square root of variance-covariance matrix)
	_Stmp = NewTwoDArrayDouble(_p, _p);

	// This vector will hold eigenvalues
	_w .resize(_p);

	// Create _z (two-dimensional array of eigenvectors)
	_z = NewTwoDArrayDouble(_p, _p);

    // This vector will hold the n parameter vectors for one tree topology
    _posterior_samples.resize(n);

#if 0
    // fill _posterior_samples with multivariate standard normal vectors for debugging tests
    Lot lot = Lot();
    double debug_sigma = 5.0;
    for (unsigned i = 0; i < _n; ++i)
        {
        PosteriorSample & sample = _posterior_samples[i];
        sample._parameters.resize(_p);
        for (unsigned j = 0; j < _p; ++j)
            {
            sample._parameters[j] = debug_sigma*lot.Normal();
            }
        }
#endif
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Destroys arrays and matrices that do not automatically destroy themselves.
*/
VarCovMatrix::~VarCovMatrix()
	{
	if (_V)
		{
		DeleteTwoDArray<double>(_V);
		_V = 0;
		}

	if (_S)
		{
		DeleteTwoDArray<double>(_S);
		_S = 0;
		}

	if (_Sinv)
		{
		DeleteTwoDArray<double>(_Sinv);
		_Sinv = 0;
		}

	if (_Stmp)
		{
		DeleteTwoDArray<double>(_Stmp);
		_Stmp = 0;
		}

	if (_z)
		{
		DeleteTwoDArray<double>(_z);
		_z = 0;
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns 0.5*logDet(), which is the log of the Jacobian for the standardization transformation (the log of the
|   determinant of the _S matrix).
*/
double VarCovMatrix::logJacobianForStandardization() const
	{
	return 0.5*logDet();
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns data member `_log_ratio_sum', which is calculated in the member function calcRepresentativeForShell.
*/
double VarCovMatrix::getLogRatioSum() const
	{
	return _log_ratio_sum;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns data member `_log_ratio_vect', which is calculated in the member function calcRepresentativeForShell.
*/
std::vector<double> VarCovMatrix::getLogRatioVect() const
	{
	return _log_ratio_vect;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_logkernel', which is calculated in the member function
|   calcRepresentativeForShell.
*/
double VarCovMatrix::getRepresentativeLogKernel() const
	{
	return _representative_logkernel;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Calculates sum of elements on the log scale, controlling for underflow, and returns sum on log scale. For example,
|   if a is the largest element of the vector {a,b,c}, then
|   sum = a + b + c
|       = a (1 + b/a + c/a)
|   log(sum) = log(a) +  log(1 + exp(log(b) - log(a)) + exp(log(c) - log(a)))
|   This approach uses only log(a), log(b) and log(c) to compute the log of the sum a + b + c.
*/
double VarCovMatrix::calcSumTermsOnLogScale(std::vector<double> & log_terms) const
    {
    assert(log_terms.size() > 0);
    double max_log_term = *std::max_element(log_terms.begin(), log_terms.end());
    double sum_of_terms = 0.0;
    BOOST_FOREACH(double logx, log_terms)
        {
        sum_of_terms += exp(logx - max_log_term);
        }
    double sum_on_log_scale = max_log_term + log(sum_of_terms);
    return sum_on_log_scale;
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_loglikelihood', which is calculated in the member function
|   calcRepresentativeForShell.
*/
double VarCovMatrix::getRepresentativeLogLikelihood() const
	{
	return _representative_loglikelihood;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_logprior', which is calculated in the member function
|   calcRepresentativeForShell.
*/
double VarCovMatrix::getRepresentativeLogPrior() const
	{
	return _representative_logprior;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_logjacobian_edgelen', which is calculated in the member
|   function calcRepresentativeForShell.
*/
double VarCovMatrix::getRepresentativeLogJacobianEdgelen() const
	{
	return _representative_logjacobian_edgelen;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_logjacobian_substmodel', which is calculated in the member
|   function calcRepresentativeForShell.
*/
double VarCovMatrix::getRepresentativeLogJacobianSubstmodel() const
	{
	return _representative_logjacobian_substmodel;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_logjacobian_standardization', which is calculated in the
|   member function calcRepresentativeForShell.
*/
double VarCovMatrix::getRepresentativeLogJacobianStandardization() const
	{
	return _representative_logjacobian_standardization;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Returns current value of the data member `_representative_param_vector', which is calculated in the member function
|   calcRepresentativeForShell.
*/
std::vector<double> VarCovMatrix::getRepresentativeParamVect() const
	{
	return _representative_param_vector;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Accessor function that returns the current value of the data member `_n', which is the number of sampled parameter
|   vectors for a single tree topology.
*/
unsigned VarCovMatrix::getNumSamples() const
	{
	return _n;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Accessor function that returns the current value of the data member `_p', which is the number of sampled parameters
|   for a single sampled tree (includes edge lengths and model parameters).
*/
unsigned VarCovMatrix::getNumParams() const
	{
	return _p;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Copies v (vector of transformed sampled parameters) to row `row' of the `_posterior_samples' vector. Also copy the
|   log likelihood (lnL), log joint prior (lnP), and log jacobian for the log (or log ratio) transformation to the
|   sample corresponding to row `row' of the `_posterior_samples' vector.
*/
void VarCovMatrix::copyParamVector(unsigned row, std::vector<double> v, double lnL, double lnP, double lnJ_edgelen, double lnJ_substmodel)
    {
    // Get object corresponding to row
    assert(_posterior_samples.size() > row);
    PosteriorSample & sample = _posterior_samples[row];

    // Copy the parameter vector
    sample._parameters.resize(v.size());
    std::copy(v.begin(), v.end(), sample._parameters.begin());

    // Copy lnL, lnP, and lnJ
    sample._log_likelihood = lnL;
    sample._log_prior = lnP;
    sample._log_jacobian_edgelen = lnJ_edgelen;
    sample._log_jacobian_substmodel = lnJ_substmodel;
    sample._log_jacobian_standardization = 0.0;  // will update this later to equal log jacobian for standardization transformation
    sample._radius = 0.0;           // cannot set this until after standardization
    }

/*----------------------------------------------------------------------------------------------------------------------
|
*/
void VarCovMatrix::setParamNames(std::vector<std::string> names)
    {
    _param_names.resize(names.size());
    std::copy(names.begin(), names.end(), _param_names.begin());
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Calculates mean vector _means, variance-covariance matrix _V, standard deviation matrix _S (and also _Sinv) and uses
|   these to standardize the parameter vectors stored in `_posterior_samples', returning Euclidian distance from the
|   origin to the furthest sample point.
*/
double VarCovMatrix::standardizeSamples()
    {
    // if _means is not empty, then standardization has already been done
    assert(_means.empty());
    assert(_mode.empty());

    // calculate means
    _means.resize(_p, 0.0);
    typedef PosteriorSample & sample_ref;
    BOOST_FOREACH(sample_ref sample, _posterior_samples)
        {
        unsigned i = 0;
        BOOST_FOREACH(double x, sample._parameters)
            {
            _means[i++] += x;
            }
        }
    std::transform(_means.begin(), _means.end(), _means.begin(), boost::lambda::_1/_n);

#if 1
    // print _means vector
    std::cerr << "\n_means = c(";
    std::copy(_means.begin(), _means.end(), std::ostream_iterator<double>(std::cerr, ","));
    std::cerr << ")" << std::endl;
#endif

    // calculate median vector
    _mode.resize(_p, 0.0);
    sample_ref best = _posterior_samples[0];
    double best_log_kernel = best._log_likelihood; // + best._log_prior + best._log_jacobian_edgelen + best._log_jacobian_substmodel + best._log_jacobian_standardization;
    BOOST_FOREACH(sample_ref sample, _posterior_samples)
        {
        double log_kernel = sample._log_likelihood; // + sample._log_prior + sample._log_jacobian_edgelen + sample._log_jacobian_substmodel + sample._log_jacobian_standardization;
        if (log_kernel > best_log_kernel)
            {
            unsigned i = 0;
            BOOST_FOREACH(double x, sample._parameters)
                {
                _mode[i++] = x;
                }
            }
        }

#if 1
    // print _mode vector
    std::cerr << "\n_mode = c(";
    std::copy(_mode.begin(), _mode.end(), std::ostream_iterator<double>(std::cerr, ","));
    std::cerr << ")" << std::endl;
#endif

    if (_center_around_mode)
        {
        // center observations around median
        BOOST_FOREACH(sample_ref sample, _posterior_samples)
            {
            unsigned i = 0;
            BOOST_FOREACH(double & x, sample._parameters)
                {
                x -= _mode[i++];
                }
            }
        }
    else
        {
        // center observations around mean
        BOOST_FOREACH(sample_ref sample, _posterior_samples)
            {
            unsigned i = 0;
            BOOST_FOREACH(double & x, sample._parameters)
                {
                x -= _means[i++];
                }
            }
        }

#if 0
    // save R file for plotting density of each log-transformed and centered parameter
    assert(_p == _param_names.size());
    std::string fn = boost::str(boost::format("%s.R") % _name);

    std::ofstream sh("plots.sh", std::ios::out | std::ios::app);
    sh << "rscript " << fn << std::endl;
    sh.close();

    std::ofstream f(fn, std::ios::out);
    f << "cwd = system('cd \"$( dirname \"$0\" )\" && pwd', intern = TRUE)" << std::endl;
    f << "setwd(cwd)" << std::endl;
    f << boost::str(boost::format("pdf(\"%s.pdf\")") % _name) << std::endl;

    for (unsigned i = 0; i < _p; ++i)
        {
        f << boost::str(boost::format("p%d = c(") % i);
        unsigned j = 0;
        BOOST_FOREACH(sample_ref sample, _posterior_samples)
            {
            if (j > 0)
                f << boost::str(boost::format(",%.5f") % sample._parameters[i]);
            else
                f << boost::str(boost::format("%.5f") % sample._parameters[i]);
            ++j;
            }
        f << ")" << std::endl;
        }
    unsigned nrows = 4;
    unsigned ncols = 3;
    f << boost::str(boost::format("par(mfrow=c(%d,%d))") % nrows % ncols) << std::endl;
    for (unsigned i = 0; i < _p; ++i)
        {
        f << boost::str(boost::format("plot(density(p%d), xlab=\"\", ylab=\"\", main=\"%s\")") % i % _param_names[i]) << std::endl;
        f << "abline(v=0)" << std::endl;
        }
    f << "dev.off()" << std::endl;
    f.close();
#endif

#if 0
    // check centering by calculating mean of centered observations
    std::vector<double> centered_means;
    centered_means.resize(_p, 0.0);
    BOOST_FOREACH(sample_ref sample, _posterior_samples)
        {
        unsigned i = 0;
        BOOST_FOREACH(double x, sample._parameters)
            {
            centered_means[i++] += x;
            }
        }
    std::transform(centered_means.begin(), centered_means.end(), centered_means.begin(), boost::lambda::_1/_n);

    // print centered_means vector
    std::cerr << "centered_means = c(";
    std::copy(centered_means.begin(), centered_means.end(), std::ostream_iterator<double>(std::cerr, ","));
    std::cerr << ")" << std::endl;
#endif

    // construct _V = (C' C)/(n-1), where C = n by p centered observations matrix
    for (unsigned i = 0; i < _p; i++)
        {
        for (unsigned j = 0; j < _p; j++)
            {
            double xsum = 0.0;
            for (unsigned k = 0; k < _n; k++)
                {
                sample_ref sample = _posterior_samples[k];
                xsum += sample._parameters[i]*sample._parameters[j];
                }
            _V[i][j] = xsum/(_n-1);
            }
        }

    // ensure that _V is symmetric
    for (unsigned i = 0; i < _p - 1; i++)
        {
        for (unsigned j = i + 1; j < _p; j++)
            {
            double tmp = (_V[i][j] + _V[j][i])/2.0;
            _V[i][j] = tmp;
            _V[j][i] = tmp;
            }
        }

    // Calculate standard deviation matrix _S and its inverse, _Sinv
    //std::cerr << "Just about to call calcEigensystem()..." << std::endl;
    calcEigensystem();

    //std::cerr << "Just about to call calcSqrtVarCov()..." << std::endl;
    calcSqrtVarCov();

    //std::cerr << "Just about to standardize _observations..." << std::endl;

    // Standardize centered _observations by premultiplying by _Sinv
    // Also, update jacobian for each observation for the standardization
    double log_det_S = 0.5*logDet();
    double max_radius = 0.0;
    for (unsigned i = 0; i < _n; i++)
        {
        sample_ref sample = _posterior_samples[i];
        sample._standardized.resize(sample._parameters.size());
        sample._log_jacobian_standardization = log_det_S;
        for (unsigned j = 0; j < _p; j++)
            {
            double xsum = 0.0;
            for (unsigned k = 0; k < _p; k++)
                {
                //xsum += sample._parameters[k]*_Sinv[k][j]; // postmultiply (wrong)
                //xsum += _Sinv[j][k]*sample._parameters[k]; // premultiply
                xsum += _Stmp[j][k]*sample._parameters[k]; // premultiply using _Stmp
                }
            sample._standardized[j] = xsum;
            }

        double r = 0.0;
        BOOST_FOREACH(double v, sample._standardized)
            {
            r += v*v;
            }
        sample._radius = sqrt(r);
        if (sample._radius > max_radius)
            max_radius = sample._radius;
        }

    return max_radius;
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Calculate the length of the supplied vector as the square root of the sum of squared elements.
*/
double VarCovMatrix::calcVectorNorm(std::vector<double> & v) const
    {
    //unsigned sz = (unsigned)v.size();
    double sum_of_squares = 0.0;
    BOOST_FOREACH(double x, v)
        {
        sum_of_squares += x*x;
        }
    return sqrt(sum_of_squares);
    }

/*----------------------------------------------------------------------------------------------------------------------
|   For debugging
*/
std::vector<double> VarCovMatrix::destandardizeSample(unsigned row)
    {
    _representative_param_vector.clear();
    _representative_param_vector.resize(_p, 0.0);

    PosteriorSample & sample = _posterior_samples[row];
    std::copy(sample._standardized.begin(), sample._standardized.end(), _representative_param_vector.begin());

    // Debugging: destandardize _representative_param_vector
    // Y' = S^{-1}*(X - Xmean) (_p x _p)*(_p x 1)
    std::vector<double> Y(_p, 0.0);
    matrixTimesVector(_S, &_representative_param_vector[0], &Y[0]);
    if (_center_around_mode)
        vectorPlusVector(&Y[0], &_mode[0], &_representative_param_vector[0]);
    else
        vectorPlusVector(&Y[0], &_means[0], &_representative_param_vector[0]);

    return _representative_param_vector;
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Returns vector of parameter vectors. The first `n' vectors are chosen, while any after that were sampled.
*/
std::vector<double> VarCovMatrix::getRepresentativesForShell(unsigned n, double r, double delta)
	{
    std::vector<double> vect_of_vects;

    Lot rng;
    std::vector<double> Y(_p, 0.0);
    std::vector<double> random_vector(_p, 0.0);
    for (unsigned i = 0; i < n; ++i)
        {
        // Choose random vector of length r by normalizing and rescaling a vector of random normal deviates
        double sumsq = 0.0;
        for (unsigned i = 0; i < _p; ++i)
            {
            double x = rng.Normal();
            random_vector[i] = x;
            sumsq += x*x;
            }
        double vector_length = sqrt(sumsq);
        std::transform(random_vector.begin(), random_vector.end(), random_vector.begin(), boost::lambda::_1*r/vector_length);

        // Destandardize vector
        matrixTimesVector(_S, &random_vector[0], &Y[0]);
        std::vector<double> v(_p, 0.0);
        if (_center_around_mode)
            vectorPlusVector(&Y[0], &_mode[0], &v[0]);
        else
            vectorPlusVector(&Y[0], &_means[0], &v[0]);
        std::copy(v.begin(), v.end(), std::back_inserter(vect_of_vects));
        }

    BOOST_FOREACH(PosteriorSample & p, _posterior_samples)
        {
        if (p._radius > r - delta && p._radius <= r + delta)
            {
            std::copy(p._standardized.begin(), p._standardized.end(), random_vector.begin());

            // Debugging: destandardize random_vector
            matrixTimesVector(_S, &random_vector[0], &Y[0]);
            std::vector<double> v(_p, 0.0);
            if (_center_around_mode)
                vectorPlusVector(&Y[0], &_mode[0], &v[0]);
            else
                vectorPlusVector(&Y[0], &_means[0], &v[0]);
            std::copy(v.begin(), v.end(), std::back_inserter(vect_of_vects));
            }
        }

    return vect_of_vects;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   If there are any samples with radius between `r' - `delta' and `r' + `delta', compute average log kernel for those
|   samples and store in `_representative_logkernel'. If not, choose random point at radius `r' and fill the vector
|   `_representative_param_vector' with the destandardized parameter values. Returns the number of samples found in the
|   shell specified by `r' and `delta' and sets `_shell_midpoint' to 'r' and `_shell_delta' to `delta'.
*/
unsigned VarCovMatrix::calcRepresentativeForShell(double r, double delta)
	{
    _shell_midpoint = r;
    _shell_delta = delta;
    _representative_param_vector.clear();
    _representative_param_vector.resize(_p, 0.0);

    // Find number of samples for this shell
    _representative_logkernel = 0.0;
    _representative_loglikelihood = 0.0;
    _representative_logprior = 0.0;
    _representative_logjacobian_edgelen = 0.0;
    _representative_logjacobian_substmodel = 0.0;
    _representative_logjacobian_standardization = 0.0;
    unsigned num_samples_this_shell = 0;
    std::vector<double> tmp_logkernelvect;
    BOOST_FOREACH(PosteriorSample & p, _posterior_samples)
        {
        if (p._radius > _shell_midpoint - _shell_delta && p._radius <= _shell_midpoint + _shell_delta)
            {
            //std::cerr << boost::str(boost::format("%6d lnL=%.5f lnP=%.5f lnJe=%.5f lnJp=%.5f lnJs=%.5f") % num_samples_this_shell % p._log_likelihood % p._log_prior % p._log_jacobian_edgelen % p._log_jacobian_substmodel % p._log_jacobian_standardization) << std::endl;
            tmp_logkernelvect.push_back(p._log_likelihood + p._log_prior + p._log_jacobian_edgelen + p._log_jacobian_substmodel + p._log_jacobian_standardization);

            _representative_logkernel += p._log_likelihood + p._log_prior + p._log_jacobian_edgelen + p._log_jacobian_substmodel + p._log_jacobian_standardization;
            num_samples_this_shell++;
            }
        }

    //std::cerr << "--> Done listing samples for this shell" << std::endl;

    if (num_samples_this_shell > 0)
        {
        // Representative log kernel value is simple average of log kernel values of samples in this shell
        _representative_logkernel /= num_samples_this_shell;

#if 0
        // Debugging: find element of _posterior_samples closest to _representative_logkernel
        PosteriorSample & closest = _posterior_samples[0];
        BOOST_FOREACH(PosteriorSample & p, _posterior_samples)
            {
            if (p._radius > _shell_midpoint - _shell_delta && p._radius <= _shell_midpoint + _shell_delta)
                {
                double plogkernel = p._log_likelihood + p._log_prior + p._log_jacobian_edgelen + p._log_jacobian_substmodel + p._log_jacobian_standardization;
                double clogkernel = closest._log_likelihood + closest._log_prior + closest._log_jacobian_edgelen + closest._log_jacobian_substmodel + closest._log_jacobian_standardization;
                if (fabs(plogkernel - _representative_logkernel) < fabs(clogkernel - _representative_logkernel))
                    {
                    closest = p;
                    }
                }
            }

        // Debugging: fill _representative_param_vector with sample closest to _representative_logkernel
        _representative_logkernel     = closest._log_likelihood + closest._log_prior + closest._log_jacobian_edgelen + closest._log_jacobian_substmodel + closest._log_jacobian_standardization;
        _representative_loglikelihood = closest._log_likelihood;
        _representative_logprior      = closest._log_prior;
        _representative_logjacobian_edgelen         = closest._log_jacobian_edgelen;
        _representative_logjacobian_substmodel      = closest._log_jacobian_substmodel;
        _representative_logjacobian_standardization = closest._log_jacobian_standardization;
        std::copy(closest._standardized.begin(), closest._standardized.end(), _representative_param_vector.begin());

        // Debugging: destandardize _representative_param_vector
        std::vector<double> Y(_p, 0.0);
        matrixTimesVector(_S, &_representative_param_vector[0], &Y[0]);
        if (_center_around_mode)
            vectorPlusVector(&Y[0], &_mode[0], &_representative_param_vector[0]);
        else
            vectorPlusVector(&Y[0], &_means[0], &_representative_param_vector[0]);

        // Spit out closest
        std::cerr << "*** comparing closest to chosen points ***" << std::endl;
        std::cerr << boost::str(boost::format("%20s %12.5f ") % "closest" % _representative_logkernel);
        BOOST_FOREACH(double v, _representative_param_vector)
            {
            std::cerr << boost::str(boost::format("%12.5f ") % v);
            }
        std::cerr << std::endl;

        // Find index of largest eigenvalue
        unsigned primary_eigenvalue_index = (unsigned)(std::max_element(_w.begin(), _w.end()) - _w.begin());

        // Copy column from eigenvector matrix representing the primary eigenvector
        std::vector<double> primary_eigenvector(_p);
        for (unsigned i = 0; i < _p; ++i)
            {
            primary_eigenvector[i] = _z[i][primary_eigenvalue_index];
            }

        // Scale primary eigenvector to have length _shell_midpoint
        std::transform(primary_eigenvector.begin(), primary_eigenvector.end(), primary_eigenvector.begin(), boost::lambda::_1*_shell_midpoint);
#pragma clang diagnostic ignored "-Wunused-variable"
        double tmp = calcVectorNorm(primary_eigenvector);
        assert(fabs(tmp - _shell_midpoint) < 1.e8);

        // First, draw point on hypersphere of radius _shell_midpoint along primary eigenvector and destandardize it
        matrixTimesVector(_S, &primary_eigenvector[0], &Y[0]);
        if (_center_around_mode)
            vectorPlusVector(&Y[0], &_mode[0], &_representative_param_vector[0]);
        else
            vectorPlusVector(&Y[0], &_means[0], &_representative_param_vector[0]);

        // Spit out point
        std::cerr << boost::str(boost::format("%20s %12s ") % "chosen" % "---");
        BOOST_FOREACH(double v, _representative_param_vector)
            {
            std::cerr << boost::str(boost::format("%12.5f ") % v);
            }
        std::cerr << std::endl;
#endif

        }
    else
        {
        // No samples available for this shell, so pick a point on the hypersphere at radius _shell_midpoint
        // and store that in _representative_param_vector so that log kernel can be computed by caller
        //double log_prior = 0.0;
        //double log_jacobian = 0.0;

        // Find index of largest eigenvalue
        unsigned primary_eigenvalue_index = (unsigned)(std::max_element(_w.begin(), _w.end()) - _w.begin());

        //std::cerr << "Eigenvalues: largest is at index " << primary_eigenvalue_index << std::endl;
        //unsigned which = 0;
        //BOOST_FOREACH(double v, _w)
        //    {
        //    std::cerr << boost::str(boost::format("%12d %12.5f") % which % v) << std::endl;
        //    which++;
        //    }
        //std::cerr << std::endl;

        // Copy column from eigenvector matrix representing the primary eigenvector
        std::vector<double> primary_eigenvector(_p);
        for (unsigned i = 0; i < _p; ++i)
            {
            primary_eigenvector[i] = _z[i][primary_eigenvalue_index];
            }

        // Scale primary eigenvector to have length _shell_midpoint
        std::transform(primary_eigenvector.begin(), primary_eigenvector.end(), primary_eigenvector.begin(), boost::lambda::_1*_shell_midpoint);
#pragma clang diagnostic ignored "-Wunused-variable"
        double tmp = calcVectorNorm(primary_eigenvector);
        assert(fabs(tmp - _shell_midpoint) < 1.e8);

        //std::cerr << "_shell_midpoint                   = " << _shell_midpoint << std::endl;
        //std::cerr << "scaled primary eigenvector length = " << tmp << std::endl;

        // First, draw point on hypersphere of radius _shell_midpoint along primary eigenvector
        // and destandardize it
        std::vector<double> Y(_p, 0.0);
        matrixTimesVector(_S, &primary_eigenvector[0], &Y[0]);
        if (_center_around_mode)
            vectorPlusVector(&Y[0], &_mode[0], &_representative_param_vector[0]);
        else
            vectorPlusVector(&Y[0], &_means[0], &_representative_param_vector[0]);
        }

    // If num_samples_this_shell > 0, then store representative kernel/sample kernel ratios
    _log_ratio_sum = 0.0;
    _log_ratio_vect.resize(num_samples_this_shell, 0.0);
    if (num_samples_this_shell > 0)
        {
        //std::vector<double> log_ratios(num_samples_this_shell, 0.0);
        unsigned i = 0;
        BOOST_FOREACH(PosteriorSample & p, _posterior_samples)
            {
            if (p._radius > _shell_midpoint - _shell_delta && p._radius <= _shell_midpoint + _shell_delta)
                {
                double log_ratio = _representative_logkernel - p._log_likelihood - p._log_prior - p._log_jacobian_edgelen - p._log_jacobian_substmodel - p._log_jacobian_standardization;
                _log_ratio_vect[i++] = log_ratio;
                }
            }
        _log_ratio_sum = calcSumTermsOnLogScale(_log_ratio_vect);

#if 0
        std::cerr << "\ncalcRepresentativeForShell: (" << (r - delta) << ", " << (r + delta) << ")" << std::endl;
        std::cerr << "log kernel values for this shell:" << std::endl;
        std::cerr << "logk <- c(";
        std::copy(tmp_logkernelvect.begin(), tmp_logkernelvect.end(), std::ostream_iterator<double>(std::cerr, ","));
        std::cerr << ")" << std::endl;
        std::cerr << "plot(density(logk))" << std::endl;
        std::cerr << "abline(v=" << _representative_logkernel << ")" << std::endl;
        std::cerr << "repr, sampled, repr-sampled, exp(repr-sampled)" << std::endl;
        std::cerr << boost::str(boost::format("%20s %20s %20s %20s") % "repr" % "sampled" % "repr-sampled" % "exp(repr-sampled)") << std::endl;
        BOOST_FOREACH(double logv, tmp_logkernelvect)
            {
            std::cerr << boost::str(boost::format("%20.5f %20.5f %20.5f %20.5f") % _representative_logkernel % logv % (_representative_logkernel - logv) % exp(_representative_logkernel - logv) ) << std::endl;
            }
        //std::cerr << "mean log kernel value for this shell:" << _representative_logkernel << std::endl;
        //std::cerr << "minimum log kernel value for this shell:" << *std::min_element(tmp_logkernelvect.begin(), tmp_logkernelvect.end()) << std::endl;
        //std::cerr << "maximum log kernel value for this shell:" << *std::max_element(tmp_logkernelvect.begin(), tmp_logkernelvect.end()) << std::endl;
#endif

        }

    return num_samples_this_shell;
}

/*----------------------------------------------------------------------------------------------------------------------
|   Computes mean of the radial error distribution (Edmundson, 1959). This is the expected distance from the origin to
|   a Normal(0, sigma^2) deviate.
*/
double VarCovMatrix::radialErrorDistMean(double sigma) const
	{

    // calculate mean of radial error distribution
    double radial_error_dist_mean = exp(0.5*log(2.0) + boost::math::lgamma<double>((_p+1.0)*0.5) - boost::math::lgamma<double>(_p*0.5));
    return radial_error_dist_mean;
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Computes _S and _Sinv from eigenvalues _w and eigenvectors _z, which are assumed to have already been calculated
|   from _V.
*/
void VarCovMatrix::calcSqrtVarCov()
	{
    // Compute _S (square root of variance-covariance matrix)
    //
    //      | _z[0][0] _z[0][1] | | sqrt(_w[0])        0       | | _z[0][0] _z[1][0] |
    // _S = |                   | |                            | |                   |
    //      | _z[1][0] _z[1][1] | |      0         sqrt(_w[1]) | | _z[0][1] _z[1][1] |
    //
    //      | _z[0][0] sqrt(_w[0]) _z[0][0] + _z[0][1] sqrt(_w[1]) _z[0][1]     _z[0][0] sqrt(_w[0]) _z[1][0] + _z[0][1] sqrt(_w[1]) _z[1][1] |
    //    = |                                                                                |
    //      | _z[1][0] sqrt(_w[0]) _z[0][0] + _z[1][1] sqrt(_w[1]) _z[0][1]     _z[1][0] sqrt(_w[0]) _z[1][0] + _z[1][1] sqrt(_w[1]) _z[1][1] |

	double * * dbl2d = NewTwoDArrayDouble(_p, _p);
    for (unsigned i = 0; i < _p; i++)
        {
        for (unsigned j = 0; j < _p; j++)
            {
            double tmp = 0.0;
            double tmp2 = 0.0;
            for (unsigned k = 0; k < _p; k++)
                {
                tmp += _z[i][k] * sqrt(_w[k]) * _z[j][k];
                tmp2 += _z[i][k] * pow(_w[k],-0.5) * _z[j][k];
                }
            _S[i][j] = tmp;
            _Stmp[i][j] = tmp2;
            dbl2d[i][j] = tmp;
            }
        }

    // Compute _Sinv (inverse of square root of variance-covariance matrix)
    std::vector<double> dbl1d(_p, 0.0);
    std::vector<int> int1d(_p, 0.0);
	int err_code = InvertMatrix(dbl2d, _p, &dbl1d[0], &int1d[0], _Sinv);
    DeleteTwoDArray<double>(dbl2d);

	if (err_code != 0)
		{
		throw XLikelihood("Error encountered when inverting the square root of the variance-covariance matrix");
		}

    if (false)
        {
        // debug check: see if _Sinv equals _Stmp
        std::cerr << "\n_Sinv - _Stmp:" << std::endl;
        double * * I = NewTwoDArrayDouble(_p, _p);
        matrixMinusMatrix(_S, _Sinv, I);
        std::cerr << showMatrix(I, _p) << std::endl;

        // debug check: see if _S multipled by _Sinv equals I
        std::cerr << "\n_S * _Sinv:" << std::endl;
        double * * J = NewTwoDArrayDouble(_p, _p);
        matrixTimesMatrix(_S, _Sinv, J);
        std::cerr << showMatrix(J, _p) << std::endl;

        // debug check: see if _S multipled by _S equals _V
        double * * V = NewTwoDArrayDouble(_p, _p);
        double * * Z = NewTwoDArrayDouble(_p, _p);
        matrixTimesMatrix(_S, _S, V);
        matrixMinusMatrix(_V, V, Z);
        std::cerr << "\n_V - _S * _S:" << std::endl;
        std::cerr << showMatrix(Z, _p) << std::endl;

        DeleteTwoDArray<double>(I);
        DeleteTwoDArray<double>(J);
        DeleteTwoDArray<double>(V);
        DeleteTwoDArray<double>(Z);
        }
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Add vector u and vector v and return the sum in vector u_plus_v.
*/
void VarCovMatrix::vectorPlusVector(double * u, double * v, double * u_plus_v)
    {
    for (unsigned i = 0; i < _p; ++i)
        {
            u_plus_v[i] = u[i] + v[i];
        }
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Multiply matrix u and matrix v and return the product in matrix u_times_v.
*/
void VarCovMatrix::matrixTimesMatrix(double * * u, double * * v, double * * u_times_v)
    {
    for (unsigned i = 0; i < _p; ++i)
        {
        for (unsigned j = 0; j < _p; ++j)
            {
            double tmp = 0.0;
            for (unsigned k = 0; k < _p; ++k)
                {
                tmp += u[i][k]*v[k][j];
                }
            u_times_v[i][j] = tmp;
            }
        }
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Multiply _p X _p matrix u by _p-length vector v and return the product in _p-length vector u_times_v.
*/
void VarCovMatrix::matrixTimesVector(double * * u, double * v, double * u_times_v)
    {
    for (unsigned i = 0; i < _p; ++i)
        {
        double tmp = 0.0;
        for (unsigned j = 0; j < _p; ++j)
            {
            tmp += u[i][j]*v[j];
            }
        u_times_v[i] = tmp;
        }
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Subtract matrix v from matrix u and return the product in matrix u_minus_v.
*/
void VarCovMatrix::matrixMinusMatrix(double * * u, double * * v, double * * u_minus_v)
    {
    for (unsigned i = 0; i < _p; ++i)
        {
        for (unsigned j = 0; j < _p; ++j)
            {
            u_minus_v[i][j] = u[i][j] - v[i][j];
            }
        }
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Add matrix u to matrix u and return the product in matrix u_plus_v.
*/
void VarCovMatrix::matrixPlusMatrix(double * * u, double * * v, double * * u_plus_v)
    {
    for (unsigned i = 0; i < _p; ++i)
        {
        for (unsigned j = 0; j < _p; ++j)
            {
            u_plus_v[i][j] = u[i][j] + v[i][j];
            }
        }
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Raise the matrix `V' to the power `power' and place the result in A, which should have already been allocated. The
|   eigenvectors and eigenvalues will be returned in the matrix `eigenvectors' and vector `eigenvalues', respectively.
*/
void VarCovMatrix::MatrixPow(double * * V, double power, unsigned p, double * * A, double * * eigenvectors, std::vector<double> & eigenvalues)
	{
    std::cerr << "\n################## A (start MatrixPow) ##################\n\n" << showMatrix(A, p) << std::endl;
    std::cerr << "\n################## V (start MatrixPow) ##################\n\n" << showMatrix(V, p) << std::endl;
    std::cerr << "\n################## eigenvectors (start MatrixPow) ##################\n\n" << showMatrix(eigenvectors, p) << std::endl;
    std::cerr << "\n################## eigenvalues (start MatrixPow) ##################\n\n" << showVector(eigenvalues, p) << std::endl;

    // Compute eigenvalues and eigenvectors
    std::vector<double> work(p, 0.0);
	int err_code = EigenRealSymmetric(p, V, &eigenvalues[0], eigenvectors, &work[0]);
	if (err_code != 0)
		{
		throw XLikelihood("Error in the calculation of eigenvectors and eigenvalues for matrix V in MatrixPow");
		}

    for (unsigned i = 0; i < p; i++)
        {
        for (unsigned j = 0; j < p; j++)
            {
            double tmp = 0.0;
            for (unsigned k = 0; k < p; k++)
                {
                tmp += eigenvectors[i][k] * pow(eigenvalues[k], power) * eigenvectors[j][k];
                }
            A[i][j] = tmp;
            }
        }

    std::cerr << "\n################## A (end MatrixPow) ##################\n\n" << showMatrix(A, p) << std::endl;
    std::cerr << "\n################## V (end MatrixPow) ##################\n\n" << showMatrix(V, p) << std::endl;
    std::cerr << "\n################## eigenvectors (end MatrixPow) ##################\n\n" << showMatrix(eigenvectors, p) << std::endl;
    std::cerr << "\n################## eigenvalues (end MatrixPow) ##################\n\n" << showVector(eigenvalues, p) << std::endl;
    }

/*----------------------------------------------------------------------------------------------------------------------
|   Generate random data, compute mean vector and variance-covariance matrix, compute eigenvalues and eigenvectors of
|   variance-covariance matrix, then draw random points from concentric shells and compare random points with original
|   data points to demonstrate similarity.
*/
void VarCovMatrix::debugTestStandardization()
	{
    Lot rng;

    std::cerr << "########## Entering debugTestStandardization() ##########" << std::endl;

    // Random vector length is p
    // Sample size is n
    // Each of the p variables will be iid Gamma(shape,scale) (mean = shape*scale)
    unsigned p = 2;
    unsigned n = 1000;
    unsigned shape = 2; // must be positive integer
    double   scale = 1.0;
    bool     verbose = true;

    // Output settings and dimensions
    std::cerr << "### p = " << p << std::endl;
    std::cerr << "### n = " << n << std::endl;
    std::cerr << "### shape = " << shape << std::endl;
    std::cerr << "### scale = " << scale << std::endl;
    std::cerr << "### verbose = " << (verbose ? "yes" : "no") << std::endl;

    // Generate n p-dimensional random vectors of log-transformed Gamma(shape, scale) deviates
    std::vector< std::vector<double> > D(n);

    // Allocate memory for eigenvalues and eigenvectors
    std::vector<double> eigenvalues(p, 0.0);
    double * * eigenvectors = NewTwoDArrayDouble(p, p);
    eigenvectors[0][0] = 1.0;
    eigenvectors[0][1] = 0.0;
    eigenvectors[1][0] = 0.0;
    eigenvectors[1][1] = 1.0;

#if 1
    std::vector<double> true_mean(p, 10.0);
    std::cerr << "\n################## V ##################\n\n" << showVector(true_mean, p) << std::endl;
    double * * V = NewTwoDArrayDouble(p, p);
    assert(V != NULL);
    V[0][0] = 1.0;
    V[0][1] = 2.0;
    V[1][0] = 2.0;
    V[1][1] = 4.0;
    std::cerr << "\n################## V ##################\n\n" << showMatrix(V, p) << std::endl;

    double * * A = NewTwoDArrayDouble(p, p);
    A[0][0] = 0.0;
    A[0][1] = 0.0;
    A[1][0] = 0.0;
    A[1][1] = 0.0;

    MatrixPow(V, 0.5, p, A, eigenvectors, eigenvalues);

    std::cerr << "\n################## A ##################\n\n" << showMatrix(A, p) << std::endl;

    for (unsigned i = 0; i < n; ++i)
        {
        D[i].resize(p);

        // Generate a bivariate normal random deviate
        double z0 = rng.Normal();
        double z1 = rng.Normal();
        D[i][0] = true_mean[0] + A[0][0]*z0 + A[0][1]*z1;
        D[i][1] = true_mean[1] + A[1][0]*z0 + A[1][1]*z1;
        //std::cerr << "\n################## D[" << i << "] ##################\n\n" << showVector(D[i], p) << std::endl;
        }
#endif

#if 0
    for (unsigned i = 0; i < n; ++i)
        {
        D[i].resize(p);
        for (unsigned j = 0; j < p; ++j)
            {
            // Use fact that sum of shape Exponential(1/scale) deviates ~ Gamma(shape, scale)
            double v = 0.0;
            for (unsigned k = 0; k < shape; ++k)
                {
                double u = rng.Uniform();
                v += -scale*log(1.0 - u);
                }
            D[i][j] = log(v);
            }
        }
#endif

#if 0
    for (unsigned i = 0; i < n; ++i)
        {
        D[i].resize(p);
        for (unsigned j = 0; j < p; ++j)
            {
            // use log-transformed Exponential(1.0) deviates instead of Gamma(shape, scale) deviates
            double u = rng.Uniform();
            double v = -1.0*log(1.0 - u);
            D[i][j] = log(v);
            }
        }
#endif

    if (verbose)
        {
        // Output data matrix D
        std::cerr << "### original data matrix D: " << std::endl;
        for (unsigned i = 0; i < n; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % D[i][j]);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;
        }

    // Compute sample mean vector
    std::vector<double> mean(p, 0.0);
    for (unsigned i = 0; i < n; ++i)
        {
        for (unsigned j = 0; j < p; ++j)
            {
            mean[j] += D[i][j];
            }
        }
    std::transform(mean.begin(), mean.end(), mean.begin(), boost::lambda::_1/n);

    if (verbose)
        {
        // Output vector of sample means
        std::cerr << "### sample means: " << std::endl;
        std::copy(mean.begin(), mean.end(), std::ostream_iterator<double>(std::cerr, " "));
        std::cerr << "\n" << std::endl;
        }

    // Center observations
    std::vector< std::vector<double> > C(n);
    for (unsigned i = 0; i < n; ++i)
        {
        C[i].resize(p);
        for (unsigned j = 0; j < p; ++j)
            {
            C[i][j] = D[i][j] - mean[j];
            }
        }

    if (verbose)
        {
        // Output centered data matrix C
        std::cerr << "### centered data matrix C: " << std::endl;
        std::vector<double> centered_means(p, 0.0);
        for (unsigned i = 0; i < n; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % C[i][j]);
                centered_means[j] += C[i][j];
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;
        std::transform(centered_means.begin(), centered_means.end(), centered_means.begin(), boost::lambda::_1/n);
        std::cerr << boost::str(boost::format("%6s") % "mean");
        for (unsigned j = 0; j < p; ++j)
            {
            std::cerr << boost::str(boost::format("%12.5f") % centered_means[j]);
            }
        std::cerr << "\n" << std::endl;
        }

    // Compute sample variance-covariance matrix S = (C' C)/(n-1)
    double * * S = NewTwoDArrayDouble(p, p);
    for (unsigned i = 0; i < p; i++)
        {
        for (unsigned j = 0; j < p; j++)
            {
            double xsum = 0.0;
            for (unsigned k = 0; k < n; k++)
                {
                xsum += C[k][i]*C[k][j];
                }
            S[i][j] = xsum/(n-1);
            }
        }

    if (verbose)
        {
        // Output variance-covariance matrix S
        std::cerr << "### variance-covariance matrix S: " << std::endl;
        std::cerr << "      ";
        for (unsigned i = 0; i < p; ++i)
            std::cerr << boost::str(boost::format("%12d") % i);
        std::cerr << std::endl;
        for (unsigned i = 0; i < p; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % S[i][j]);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;
        }

    // Compute eigenvalues and eigenvectors
    std::vector<double> work(p, 0.0);
	int err_code = EigenRealSymmetric(p, S, &eigenvalues[0], eigenvectors, &work[0]);
	if (err_code != 0)
		{
		throw XLikelihood("Error in the calculation of eigenvectors and eigenvalues for matrix S in debugTestStandardization");
		}

    if (verbose)
        {
        // Output vector of eigenvalues
        std::cerr << "### eigenvalues: " << std::endl;
        std::copy(eigenvalues.begin(), eigenvalues.end(), std::ostream_iterator<double>(std::cerr, " "));
        std::cerr << "\n" << std::endl;

        // Output matrix of eigenvectors
        std::cerr << "### eigenvectors: " << std::endl;
        for (unsigned i = 0; i < p; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % eigenvectors[i][j]);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;
        }

    // Compute sqrtS (square root of variance-covariance matrix) and sqrtSinv (inverse square root of var-cov matrix).
    //
    //         | z[0][0] z[0][1] | | sqrt(w[0])        0       | | z[0][0] z[1][0] |
    // sqrtS = |                 | |                           | |                 |
    //         | z[1][0] z[1][1] | |      0         sqrt(w[1]) | | z[0][1] z[1][1] |
    //
    //         | z[0][0] sqrt(w[0]) z[0][0] + z[0][1] sqrt(w[1]) z[0][1]     z[0][0] sqrt(w[0]) z[1][0] + z[0][1] sqrt(w[1]) z[1][1] |
    //       = |                                                                                |
    //         | z[1][0] sqrt(w[0]) z[0][0] + z[1][1] sqrt(w[1]) z[0][1]     z[1][0] sqrt(w[0]) z[1][0] + z[1][1] sqrt(w[1]) z[1][1] |
    //
    // Note: z,w substituted above for eigenvectors,eigenvalues for clarity.
	double * * sqrtS = NewTwoDArrayDouble(p, p);
	double * * sqrtSinv = NewTwoDArrayDouble(p, p);
    for (unsigned i = 0; i < p; i++)
        {
        for (unsigned j = 0; j < p; j++)
            {
            double tmp1 = 0.0;
            double tmp2 = 0.0;
            for (unsigned k = 0; k < p; k++)
                {
                tmp1 += eigenvectors[i][k] * pow(eigenvalues[k], 0.5) * eigenvectors[j][k];
                tmp2 += eigenvectors[i][k] * pow(eigenvalues[k],-0.5) * eigenvectors[j][k];
                }
            sqrtS[i][j]    = tmp1;
            sqrtSinv[i][j] = tmp2;
            }
        }

    if (verbose)
        {
        // Output sqrtS matrix
        std::cerr << "### square root of variance-covariance matrix sqrtS: " << std::endl;
        for (unsigned i = 0; i < p; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % sqrtS[i][j]);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;

        // Output sqrtSinv matrix
        std::cerr << "### inverse of square root of variance-covariance matrix sqrtSinv: " << std::endl;
        for (unsigned i = 0; i < p; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % sqrtSinv[i][j]);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;

        // Output sqrtS * sqrtSinv = identity matrix
        std::cerr << "### sqrtS * sqrtSinv = I (identity matrix): " << std::endl;
        for (unsigned i = 0; i < p; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                double xsum = 0.0;
                for (unsigned k = 0; k < p; ++k)
                    {
                    xsum += sqrtS[i][k] * sqrtSinv[k][j];
                    }
                std::cerr << boost::str(boost::format("%12.5f") % xsum);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;

        // Output sqrtS * sqrtS = S (variance covariance matrix)
        std::cerr << "### sqrtS * sqrtS = S (variance-covariance matrix): " << std::endl;
        for (unsigned i = 0; i < p; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                double xsum = 0.0;
                for (unsigned k = 0; k < p; ++k)
                    {
                    xsum += sqrtS[i][k] * sqrtS[k][j];
                    }
                std::cerr << boost::str(boost::format("%12.5f") % xsum);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;
        }

    // Standardize ith centered observation vector by premultiplying by sqrtSinv
    //
    // | Zi0 |   | s00 s01 s02 | | Ci0 |   | s00 Ci0 + s01 Ci1 + s02 Ci2 |
    // |     |   |             | |     |   |                             |
    // | Zi1 | = | s10 s11 s12 | | Ci1 | = | s10 Ci0 + s11 Ci1 + s12 Ci2 |
    // |     |   |             | |     |   |                             |
    // | Zi2 |   | s20 s21 s22 | | Ci2 |   | s20 Ci0 + s21 Ci1 + s22 Ci2 |
    //                                        jk  ik    jk  ik    jk  ik
    std::vector< std::vector<double> > Z(n);
    for (unsigned i = 0; i < n; i++)
        {
        Z[i].resize(p, 0.0);
        for (unsigned j = 0; j < p; j++)
            {
            double xsum = 0.0;
            for (unsigned k = 0; k < p; k++)
                {
                xsum += sqrtSinv[j][k]*C[i][k];
                }
            Z[i][j] = xsum;
            }
        }

    if (verbose)
        {
        // Output standardized data matrix Z
        std::cerr << "### standardized data matrix Z: " << std::endl;
        for (unsigned i = 0; i < n; ++i)
            {
            std::cerr << boost::str(boost::format("%6d") % i);
            for (unsigned j = 0; j < p; ++j)
                {
                std::cerr << boost::str(boost::format("%12.5f") % Z[i][j]);
                }
            std::cerr << std::endl;
            }
        std::cerr << std::endl;
        }

    // Calculate radius for each observed point
    std::vector<double> radius(n, 0.0);
    double max_radius = 0.0;
    for (unsigned i = 0; i < n; i++)
        {
        double r = 0.0;
        for (unsigned j = 0; j < p; j++)
            {
            r += pow(Z[i][j], 2.0);
            }
        radius[i] = sqrt(r);
        if (radius[i] > max_radius)
            max_radius = radius[i];
        }

    if (verbose)
        {
        // Output radius for each point
        std::cerr << "### vector of radii (maximum = " << max_radius << "): " << std::endl;
        for (unsigned i = 0; i < n; ++i)
            {
            std::cerr << boost::str(boost::format("%6d %12.5f") % i % radius[i]) << std::endl;
            }
        std::cerr << std::endl;
        }

    // For each observed radius, make up a new point having that radius
    std::vector<double> random_vector(p, 0.0);
	double * * Q = NewTwoDArrayDouble(n, p);
    for (unsigned i = 0; i < n; ++i)
        {
        // Choose random vector of length radius[i] by normalizing and rescaling a vector of random normal deviates
        double sumsq = 0.0;
        for (unsigned j = 0; j < p; ++j)
            {
            double x = rng.Normal();
            random_vector[j] = x;
            sumsq += x*x;
            }
        double vector_length = sqrt(sumsq);
        std::transform(random_vector.begin(), random_vector.end(), random_vector.begin(), boost::lambda::_1*radius[i]/vector_length);

        // Destandardize using (sqrtS * random_vector + mean) and store in Q[i]
        for (unsigned j = 0; j < p; ++j)
            {
            double tmp = 0.0;
            for (unsigned k = 0; k < p; ++k)
                {
                tmp += sqrtS[j][k]*random_vector[k];
                }
            Q[i][j] = tmp + mean[j];
            }
        }

    std::cerr << "Saving plot data..." << std::endl;

    // Compare original to chosen points at different radii
    std::ofstream outf("standardization-test.R", std::ios::out);
    outf << "# p are the original points" << std::endl;
    outf << "# q are the simulated points" << std::endl;

    for (unsigned j = 0; j < p; ++j)
        {
        // Output the original points as p0, p1, ...
        outf << "p" << j << " <- c(" << D[0][j];
        for (unsigned i = 1; i < n; ++i)
            {
            outf << "," << D[i][j];
            }
        outf << ")\n";

        // Output the simulated points as q0, q1, ...
        outf << "q" << j << " <- c(" << Q[0][j];
        for (unsigned i = 1; i < n; ++i)
            {
            outf << "," << Q[i][j];
            }
        outf << ")\n";
        }

    // Output the radius of each point
    outf << "#radius <- c(" << radius[0];
    for (unsigned i = 1; i < n; ++i)
        {
        outf << "," << radius[i];
        }
    outf << ")\n";

    outf << "#plot(density(radius),col=\"black\")\n";
    outf << "#rug(radius)\n";
    outf << std::endl;
    outf << "#plot(density(p0),col=\"black\")\n";
    outf << "#lines(density(q0),col=\"black\")\n";
    outf << "#lines(density(p1),col=\"red\")\n";
    outf << "#lines(density(q1),col=\"red\")\n";
    outf << std::endl;
    outf << "plot(p0, p1, type=\"p\",col=\"black\")\n";
    outf << "points(q0, q1, col=\"red\")\n";
    outf << std::endl;
    outf.close();

    if (verbose)
        std::cerr << "Cleaning up..." << std::endl;

    // Clean up
    //DeleteTwoDArray<double>(V);
    DeleteTwoDArray<double>(Q);
    DeleteTwoDArray<double>(S);
    DeleteTwoDArray<double>(sqrtS);
    DeleteTwoDArray<double>(sqrtSinv);
    DeleteTwoDArray<double>(eigenvectors);

    std::cerr << "########## Leaving debugTestStandardization() ##########" << std::endl;
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Recomputes eigenvalues and eigenvectors corresponding to the following contrived variance-covariance matrix:
|
|       | 2  0  0 |
|   V = | 0  3  4 |
|       | 0  4  9 |
|
|   Eigenvalues:
|
|      1    2     11
|
|   Eigenvector matrix:    Normalized eigenvectors:
|
|      0    1      0        0.000000000  1.000000000 0.000000000
|      2    0      1        0.894427191  0.000000000 0.447213595
|     -1    0      2       -0.447213595  0.000000000 0.894427191
|
|   S = V^{0.5}
|
|   Eigenvalues:
|
|      1    1.414213562     3.31662479
|
|   Eigenvector matrix:    Normalized eigenvectors:
|
|      0    1      0        0.000000000  1.000000000 0.000000000
|      2    0      1        0.894427191  0.000000000 0.447213595
|     -1    0      2       -0.447213595  0.000000000 0.894427191
|
|   Here is the output produced:
|
|   A:
|                           0            1            2
|              0      2.00000      0.00000      0.00000
|              1      0.00000      3.00000      4.00000
|              2      0.00000      4.00000      9.00000
|
|   Eigenvalues:
|   2 1 11
|   Eigenvector matrix:
|                           0            1            2
|              0      1.00000      0.00000      0.00000
|              1      0.00000     -0.89443      0.44721
|              2      0.00000      0.44721      0.89443|
|
|   See https://en.wikipedia.org/wiki/Eigenvalues_and_eigenvectors for a detailed work through of this example.
*/
void VarCovMatrix::debugEigensystem() const
	{
    double * * A = NewTwoDArrayDouble(3, 3);
    A[0][0] = 2.0;
    A[0][1] = 0.0;
    A[0][2] = 0.0;
    A[1][0] = 0.0;
    A[1][1] = 3.0;
    A[1][2] = 4.0;
    A[2][0] = 0.0;
    A[2][1] = 4.0;
    A[2][2] = 9.0;

    std::cerr << "\n~~~~~~~~~ begin debugEigensystem ~~~~~~~~~" << std::endl;
    std::cerr << "A:\n" << showMatrix(A, 3) << std::endl;

    std::vector<double> fv(3, 0.0);
    std::vector<double> v(3, 0.0);
    double * * g = NewTwoDArrayDouble(3, 3);

	// Calculate eigenvalues (v) and eigenvectors (g) (fv is just workspace)
	int err_code = EigenRealSymmetric(3, A, &v[0], g, &fv[0]);

	if (err_code != 0)
		{
		throw XLikelihood("Error in the calculation of eigenvectors and eigenvalues for matrix A in debugEigensystem");
		}

    std::cerr << "Eigenvalues:" << std::endl;
    std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cerr, " "));
    std::cerr << "\nEigenvector matrix:\n" << showMatrix(g, 3) << std::endl;

#if 0
    // Now calculate the matrix B, square root of A
	double * * B = NewTwoDArrayDouble(3, 3);
    for (unsigned i = 0; i < 3; i++)
        {
        for (unsigned j = 0; j < 3; j++)
            {
            double tmp = 0.0;
            for (unsigned k = 0; k < 3; k++)
                {
                tmp += g[i][k] * sqrt(v[k]) * g[j][k];
                }
            B[i][j] = tmp;
            }
        }

	// Calculate eigenvalues (v) and eigenvectors (g) of B
	err_code = EigenRealSymmetric(3, B, &v[0], g, &fv[0]);

	if (err_code != 0)
		{
		throw XLikelihood("Error in the calculation of eigenvectors and eigenvalues for matrix B in debugEigensystem");
		}

    std::cerr << "Eigenvalues:" << std::endl;
    std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cerr, " "));
    DeleteTwoDArray<double>(B);
#endif

    std::cerr << "\n~~~~~~~~~ end debugEigensystem ~~~~~~~~~" << std::endl;

    DeleteTwoDArray<double>(A);
    DeleteTwoDArray<double>(g);
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Recomputes eigenvalues and eigenvectors corresponding to the variance-covariance matrix.
*/
void VarCovMatrix::calcEigensystem()
	{
    //std::cerr << "\n~~~~~~~~~ begin ~~~~~~~~~" << _p << std::endl;
    //std::cerr << "_p = " << _p << std::endl;
    //std::cerr << "_V:\n" << showMatrix(_V, _p) << std::endl;
    //std::cerr << "\n~~~~~~~~~ end ~~~~~~~~~" << _p << std::endl;

	// Calculate eigenvalues (_w) and eigenvectors (_z)
    std::vector<double> fv(_p, 0.0);
	int err_code = EigenRealSymmetric(_p, _V, &_w[0], _z, &fv[0]);

	if (err_code != 0)
		{
		throw XLikelihood("Error in the calculation of eigenvectors and eigenvalues of the variance-covariance matrix");
		}
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the eigenvalues in the data member `_w' as a vector.
*/
std::vector<double> VarCovMatrix::getEigenValues() const
	{
	return _w;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the sum of log eigenvalues of the variance-covariance matrix, which equals the log of the determinant of the
|   variance-covariance matrix.
*/
double VarCovMatrix::logDet() const
    {
    double logdet = 0.0;

    for (unsigned i = 0; i < _p; ++i)
        {
        logdet += log(_w[i]);
        }

    return logdet;
    }

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the eigenvectors in the data member `_z' as a NumArray. Calls recalcQMatrix first to ensure that `_z' is up
|	to date.
*/
std::vector<double> VarCovMatrix::getEigenVectors() const
	{
	std::vector<double> p;
	flattenTwoDMatrix(p, _z, _p);
	return p;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Creates a string that, when printed, formats `V' nicely for output.
*/
std::string VarCovMatrix::showVector(std::vector<double> & V, unsigned p) const
	{
    //std::strstream ss;
    //std::copy(V.begin(), V.end(), std::ostream_iterator<double>(ss, " "));
    //return ss.str();
    std::string s;
    for (unsigned i = 0; i < p; ++i)
        s += boost::str(boost::format("%.5f ") % V[i]);
    return s;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Creates a string that, when printed, formats `V' nicely for output.
*/
std::string VarCovMatrix::showMatrix(double * * V, unsigned p) const
	{
	double * V_begin = &V[0][0];
	return showMatrixImpl(V_begin, p);
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Creates a string that, when printed, formats the matrix of eigenvectors nicely for output.
*/
std::string VarCovMatrix::showEigenVectorsAsMatrix() const
	{
	double * z_begin = &_z[0][0];
	return showMatrixImpl(z_begin, _p);
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Creates a string that, when printed, formats the matrix of eigenvectors nicely for output.
*/
std::string VarCovMatrix::showMatrixImpl(const double * q, unsigned p) const
	{
	unsigned i, j;

	// Output one column for row labels and a label for every column of _V
	std::string s = str(boost::format("%12s ") % " ");
	for (i = 0; i < p; ++i)
		{
		s += str(boost::format("%12d ") % i);
		}
	s += "\n";

	// Output rows of q
	double * pq = (double *)q;	//PELIGROSO
	for (i = 0; i < p; ++i)
		{
		s += str(boost::format("%12d ") % i);
		for (j = 0; j < p; ++j)
			{
			s += str(boost::format("%12.5f ") % *pq++);
			}
		s += "\n";
		}

	return s;
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Stores a flattened version of the supplied 2-dimensional array `twoDarr', storing the result in the supplied std::vector<double>
|	reference variable `p'. The supplied `twoDarr' should be laid out so that rows occupy contiguous memory.
*/
void VarCovMatrix::flattenTwoDMatrix(std::vector<double> & p, double * * twoDarr, unsigned dim) const
	{
	unsigned flat_length = dim*dim;
	p.resize(flat_length);
	double * twoD_begin = &twoDarr[0][0];
	double * twoD_end   = twoD_begin + flat_length;
	std::copy(twoD_begin, twoD_end, p.begin());
	}

/*----------------------------------------------------------------------------------------------------------------------
|	Returns the entries in the data member `_V' as a single dimensional vector.
*/
std::vector<double> VarCovMatrix::getVarCovMatrix() const
	{
	std::vector<double> p;
	flattenTwoDMatrix(p, _V, _p);
	return p;
	}

/*----------------------------------------------------------------------------------------------------------------------
|   Copies parameter types from `ptypes' to `_param_type' vector.
*/
//void VarCovMatrix::copyParamTypes(std::vector<unsigned> & ptypes)
//	{
//    _param_type.resize(ptypes.size());
//    std::copy(ptypes.begin(), ptypes.end(), _param_type.begin());
//	}

/*----------------------------------------------------------------------------------------------------------------------
|   Copies supplied newick tree description to `_newick'.
*/
//void VarCovMatrix::copyNewick(std::string newick)
//	{
//    _newick = newick;
//	}

/*----------------------------------------------------------------------------------------------------------------------
|   Debugging check: calculates log-likelihoods of all sampled trees for comparison with those from the original
|   parameter file.
*/
//void VarCovMatrix::debugCheckLogLikelihoods()
//	{
//	}


#include "..\..\include\sbs\physics\tetrahedral_mesh_boundary.h"
#include "..\..\include\sbs\physics\tetrahedral_mesh_boundary.h"
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <numeric>
#include <sbs/physics/tetrahedral_mesh_boundary.h>
#include <sbs/physics/topology.h>

namespace sbs {
namespace physics {

/**
 * Surface mesh adapter
 */

tetrahedral_mesh_boundary_t::tetrahedral_mesh_boundary_t(tetrahedron_set_t* mesh)
    : mesh_(mesh),
      vertex_index_map_{},
      tet_to_surface_vertex_index_map_{},
      triangle_index_map_{},
      vertices_{},
      triangles_{}
{
    extract_boundary_surface();
}

std::size_t tetrahedral_mesh_boundary_t::triangle_count() const
{
    return triangles_.size();
}

std::size_t tetrahedral_mesh_boundary_t::vertex_count() const
{
    return vertices_.size();
}

common::shared_vertex_surface_mesh_i::vertex_type
tetrahedral_mesh_boundary_t::vertex(std::size_t vi) const
{
    return vertices_[vi];
}

common::shared_vertex_surface_mesh_i::triangle_type
tetrahedral_mesh_boundary_t::triangle(std::size_t fi) const
{
    return triangles_[fi];
}

std::vector<index_type> const&
tetrahedral_mesh_boundary_t::surface_to_tetrahedral_mesh_index_map() const
{
    return vertex_index_map_;
}

index_type tetrahedral_mesh_boundary_t::from_surface_vertex(std::size_t vi) const
{
    return vertex_index_map_[vi];
}

index_type tetrahedral_mesh_boundary_t::from_surface_triangle(std::size_t fi) const
{
    return triangle_index_map_[fi];
}

void tetrahedral_mesh_boundary_t::extract_boundary_surface()
{
    vertex_index_map_.clear();
    triangle_index_map_.clear();
    tet_to_surface_vertex_index_map_.clear();
    vertices_.clear();
    triangles_.clear();

    std::size_t const tet_mesh_vertex_count = mesh_->vertex_count();
    std::size_t const tet_mesh_face_count   = mesh_->triangle_count();

    // maps tetrahedral mesh vertices to surface mesh vertices
    tet_to_surface_vertex_index_map_.resize(tet_mesh_vertex_count);

    // pre-allocate vertex storage heuristically, and triangle storage exactly
    vertex_index_map_.reserve(mesh_->triangle_count());
    triangle_index_map_.reserve(mesh_->triangle_count());
    triangles_.reserve(mesh_->triangle_count());
    vertices_.reserve(mesh_->vertex_count());

    std::vector<triangle_t> const& triangles = mesh_->triangles();
    for (std::size_t fi = 0u; fi < triangles.size(); ++fi)
    {
        bool const is_interior_triangle = triangles[fi].incident_tetrahedron_indices().size() == 2u;
        if (is_interior_triangle)
            continue;

        triangle_index_map_.push_back(static_cast<index_type>(fi));

        triangle_t const& boundary_triangle = triangles[fi];
        triangle_type surface_mesh_triangle{};

        for (std::size_t j = 0u; j < boundary_triangle.vertex_indices().size(); ++j)
        {
            index_type const vi = boundary_triangle.vertex_indices()[j];

            if (!tet_to_surface_vertex_index_map_[vi].has_value())
            {
                // update map
                index_type const new_vertex_index =
                    static_cast<index_type>(vertex_index_map_.size());
                tet_to_surface_vertex_index_map_[vi] = new_vertex_index;
                vertex_index_map_.push_back(vi);
                // Note: Vertex attributes should be set by the owning body in update_visual_model()
                vertices_.push_back({});
                surface_mesh_triangle.vertices[j] = new_vertex_index;
            }
            else
            {
                surface_mesh_triangle.vertices[j] = tet_to_surface_vertex_index_map_[vi].value();
            }
        }

        triangles_.push_back(surface_mesh_triangle);
    }
}

void tetrahedral_mesh_boundary_t::compute_normals()
{
    for (std::size_t i = 0u; i < triangles_.size(); ++i)
    {
        auto const v1 = triangles_[i].vertices[0u];
        auto const v2 = triangles_[i].vertices[1u];
        auto const v3 = triangles_[i].vertices[2u];

        Eigen::Vector3d const& p1 = vertices_[v1].position;
        Eigen::Vector3d const& p2 = vertices_[v2].position;
        Eigen::Vector3d const& p3 = vertices_[v3].position;

        Eigen::Vector3d const n = (p2 - p1).cross(p3 - p1);

        vertices_[v1].normal += n;
        vertices_[v2].normal += n;
        vertices_[v3].normal += n;
    }

    for (std::size_t i = 0u; i < vertices_.size(); ++i)
    {
        vertices_[i].normal.normalize();
    }
}

void tetrahedral_mesh_boundary_t::prepare_vertices_for_rendering()
{
    prepare_vertices_for_surface_rendering();
}

void tetrahedral_mesh_boundary_t::prepare_indices_for_rendering()
{
    prepare_indices_for_surface_rendering();
}

common::shared_vertex_surface_mesh_i::vertex_type&
tetrahedral_mesh_boundary_t::mutable_vertex(std::size_t vi)
{
    return vertices_[vi];
}

common::shared_vertex_surface_mesh_i::triangle_type&
tetrahedral_mesh_boundary_t::mutable_triangle(std::size_t f)
{
    return triangles_[f];
}

void tetrahedral_mesh_boundary_t::prepare_vertices_for_surface_rendering()
{
    std::size_t constexpr num_attributes_per_vertex = 9u;
    std::size_t const vertex_count                  = vertices_.size();
    std::vector<float> vertex_buffer{};
    vertex_buffer.reserve(vertex_count * num_attributes_per_vertex);

    for (vertex_type const& vertex : vertices_)
    {
        vertex_buffer.push_back(static_cast<float>(vertex.position.x()));
        vertex_buffer.push_back(static_cast<float>(vertex.position.y()));
        vertex_buffer.push_back(static_cast<float>(vertex.position.z()));

        // use triangle normal since face-based
        vertex_buffer.push_back(static_cast<float>(vertex.normal.x()));
        vertex_buffer.push_back(static_cast<float>(vertex.normal.y()));
        vertex_buffer.push_back(static_cast<float>(vertex.normal.z()));

        vertex_buffer.push_back(vertex.color.x());
        vertex_buffer.push_back(vertex.color.y());
        vertex_buffer.push_back(vertex.color.z());
    }

    transfer_vertices_for_rendering(std::move(vertex_buffer));
}

void tetrahedral_mesh_boundary_t::prepare_indices_for_surface_rendering()
{
    std::size_t const index_count = triangles_.size() * 3u;
    std::vector<std::uint32_t> index_buffer{};
    index_buffer.reserve(index_count);
    for (triangle_type const& f : triangles_)
    {
        index_buffer.push_back(f.vertices[0]);
        index_buffer.push_back(f.vertices[1]);
        index_buffer.push_back(f.vertices[2]);
    }

    transfer_indices_for_rendering(std::move(index_buffer));
}

void tetrahedral_mesh_boundary_t::prepare_vertices_for_wireframe_rendering()
{
    // no-op
}

void tetrahedral_mesh_boundary_t::prepare_indices_for_wireframe_rendering()
{
    // no-op
}

tetrahedron_set_t const* tetrahedral_mesh_boundary_t::tetrahedral_mesh() const
{
    return mesh_;
}

tetrahedron_set_t* tetrahedral_mesh_boundary_t::tetrahedral_mesh()
{
    return mesh_;
}

} // namespace physics
} // namespace sbs


#pragma once

#include "ros/ros.h"

#include "Common/Math/Vec3.hpp"
#include "Common/Math/Trajectory.hpp"
#include "Components/TrajectoryGenerator/RapidTrajectoryGenerator.hpp"

#include "Common/DataTypes/RadioTypes.hpp"
#include "Common/DataTypes/TelemetryPacket.hpp"
#include "Common/Time/HardwareTimer.hpp"
#include "Components/Offboard/MocapStateEstimator.hpp"
#include "Components/Offboard/QuadcopterController.hpp"
#include "Components/Offboard/SafetyNet.hpp"
#include "Components/Logic/QuadcopterConstants.hpp"
#include "Components/DepthImagePlanner/DepthImagePlanner.hpp"

#include "hiperlab_rostools/mocap_output.h"
#include "hiperlab_rostools/estimator_output.h"
#include "hiperlab_rostools/radio_command.h"
#include "hiperlab_rostools/joystick_values.h"
#include "hiperlab_rostools/telemetry.h"

#include <memory>
#include <mutex>
#include <Eigen/Dense>
#include <stdio.h>
#include <unistd.h>
#include <boost/program_options.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>
#include <ctime>
#include <opencv2/opencv.hpp>
#include <random>
#include <string>

// Code for image publisher
#include <image_transport/image_transport.h>
#include <opencv2/highgui/highgui.hpp>
#include <cv_bridge/cv_bridge.h>
#include <sensor_msgs/image_encodings.h>

// Message for debugging purpose
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include "hiperlab_rostools/planner_diagnostics.h"
#include "hiperlab_rostools/controller_diagnostics.h"

// Code for VIO
#include <nav_msgs/Odometry.h>
namespace Offboard {

// Tool to translate our vector to ros geometry msg
inline geometry_msgs::Vector3 Vec3dToVector3(const Vec3d &rhs) {
  geometry_msgs::Vector3 lhs;
  lhs.x = rhs.x;
  lhs.y = rhs.y;
  lhs.z = rhs.z;
  return lhs;
}

inline geometry_msgs::Quaternion RotationdToQuaternion(const Rotationd &rhs) {
  geometry_msgs::Quaternion lhs;
  lhs.w = rhs[0];
  lhs.x = rhs[1];
  lhs.y = rhs[2];
  lhs.z = rhs[3];
  return lhs;
}

inline geometry_msgs::Quaternion RotationfToQuaternion(const Rotationf &rhs) {
  geometry_msgs::Quaternion lhs;
  lhs.w = rhs[0];
  lhs.x = rhs[1];
  lhs.y = rhs[2];
  lhs.z = rhs[3];
  return lhs;
}

class ExampleVehicleStateMachine {
 public:
  enum FlightStage {
    StageWaitForStart,
    StageSpoolUp,
    StageTakeoff,
    StageHover,
    StageFlight,
    StageLanding,
    StageComplete,
    StageEmergency,
  };

  ExampleVehicleStateMachine();

  bool GetIsEstInitialized(void) const {
    return _est->GetIsInitialized();
  }

  bool GetIsReadyToExit(void) const {
    return _vehicleIsReadyForProgramToExit;
  }

  void Initialize(int id, std::string name, ros::NodeHandle &n,
                  BaseTimer *timer, double systemLatencyTime);

  void CallbackEstimator(const hiperlab_rostools::mocap_output &msg);
  void CallbackTelemetry(const hiperlab_rostools::telemetry &msg);
  void CallbackDepthImages(const sensor_msgs::ImageConstPtr &msg);
  void CallbackOdometry(const nav_msgs::Odometry &msg);

  void CallbackRGBImages(const sensor_msgs::ImageConstPtr &msg);

  void Run(bool shouldStart, bool shouldStop);

  void PublishEstimate(MocapStateEstimator::MocapEstimatedState estState);

  hiperlab_rostools::radio_command RunControllerAndUpdateEstimator(
      MocapStateEstimator::MocapEstimatedState estStatCameraSubscribere,
      Vec3d desPos, Vec3d desVel, Vec3d desAcc);

  hiperlab_rostools::radio_command RunTrackingControllerAndUpdateEstimator(
      MocapStateEstimator::MocapEstimatedState estState, Vec3d refPos,
      Vec3d refVel, Vec3d refAcc, double refThrust, Vec3d refAngVel,
      double &cmdThrustOutput, Rotationf &cmdAttOutput, Vec3d &cmdAngVelOutput);

  void SetDesiredPosition(Vec3d newPos) {
    _desiredPosition = newPos;
  }

  void SetDesiredYaw(double newYaw) {
    _desiredYawAngle = newYaw;
  }

  void SetExternalPanic() {
    _safetyNet->SetUnsafe();
  }

  static double GetTrajCostWrapper(
      void *ptr2obj,
      RapidQuadrocopterTrajectoryGenerator::RapidTrajectoryGenerator &traj) {
    ExampleVehicleStateMachine *veh = (ExampleVehicleStateMachine*) ptr2obj;
    return veh->GetTrajCost(traj);
  }

  double GetTrajCost(
      RapidQuadrocopterTrajectoryGenerator::RapidTrajectoryGenerator &traj) {
    double duration = traj.GetFinalTime();
    Vec3d Pi_C = traj.GetPosition(duration);  // position of camera at end of trajectory relative to start written in camera frame

    Vec3d G_W = _goalWorld;                               // goal in world frame
    Vec3d G_C = _depthCamAtt.Inverse() * _estAtt.Inverse()
        * (_goalWorld - _estPos);  // goal in camera frame
    Vec3d S_C = Vec3d(0, 0, 0);                // starting point in camera frame

    double SG = (G_C - S_C).GetNorm2();
    double PiG = (G_C - Pi_C).GetNorm2();
    return -(SG - PiG) / duration;
  }

 private:
  bool HaveLowBattery() const {
    return _lastTelWarnings & TelemetryPacket::WARN_LOW_BATT;
  }
  int _id;
  std::shared_ptr<MocapStateEstimator> _est;  // This is be a misnomer as we will be using tracking camera for correction update instead of mocap
  double _systemLatencyTime;  //[s] as used by estimator
  std::shared_ptr<QuadcopterController> _ctrl;
  std::shared_ptr<SafetyNet> _safetyNet;
  std::string _name;
  std::shared_ptr<ros::Subscriber> _subMocap, _subTelemetry, _subOdometry;
  std::shared_ptr<ros::Publisher> _pubEstimate, _pubCmd, _pubPlannerDiagnotics,
      _pubControllerDiagnotics;

  image_transport::Subscriber _subDepthImages;
  image_transport::Subscriber _subRGBImages;

//state info:
  FlightStage _flightStage, _lastFlightStage;
  std::shared_ptr<Timer> _stageTimer;  //keep track of time we've been in current stage

  volatile uint8_t _lastTelWarnings;

  Vec3d _initPosition;
  Vec3d _desiredPosition;
  double _desiredYawAngle;

  bool _vehicleIsReadyForProgramToExit;

  // Values from previous time step / stage
  Vec3d _lastPos;
  Vec3d _lastVel;
  Vec3d _lastAcc;
  double _lastYaw;
  double _cmdYawAngle;  // Might want to use this for some trajectories

  // Variables for depth image planner
  double _depthScale;
  Rotationd _depthCamAtt;
  double _focalLength;
  unsigned _depthImageHeight;
  unsigned _depthImageWidth;
  Rotationd _trajAtt;

  unsigned _depthImageCount;

  // Variables for RGB image planner
  double _rgbScale;
  Rotationd _rgbCamAtt;
  double _rgbFocalLength;
  unsigned _rgbImageHeight;
  unsigned _rgbImageWidth;
  Rotationd _rgbTrajAtt;

  unsigned _rgbImageCount;

  //This estimation global variables will be used for planning
  Rotationd _estAtt;
  Vec3d _estPos;
  Vec3d _estVel;  //In world frame
  Vec3d _estAngVel;  //In body frame

  bool _imageReady;
  bool _startPlan;
  bool _firstTrajReady;
  unsigned _plannedTrajCount;
  double _physicalVehicleRadius;
  double _vehicleRadiusPlanning;
  double _minCollisionDist;
  double _previousThrust;
  Vec3d _trajOffset;  //Offset position of the the traj from camera frame to world frame
  Vec3d _goalWorld;
  std::vector<RectangularPyramidPlanner::TrajectoryTest> _depthPlannedTraj;
  std::shared_ptr<RapidQuadrocopterTrajectoryGenerator::RapidTrajectoryGenerator> _plannedTraj;
  std::shared_ptr<Timer> _timeOnPlannedTraj;  //keep track of time we've been on the current trajectory.

  // Variables keep track of the planned trajectories
  double _plannedTrajDuration;
  double _lookAheadTime;

  std::ofstream _imageReceiveLog;
  std::ofstream _plannedTrajLog;

  ros::Time _trajResetTime;

  // Debugging thread publisher

};

}  // namespace Offboard


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

// This project is hosted at https://github.com/helo2804/css_study

#include "Domain.hpp"
#include "Format.h"

#include <cassert>
#include <cstdarg>
#include <cstdio>
#include <cstdlib>
#include <iomanip> // setfill,setw
#include <limits> // std::numeric_limits
#include <sstream>

#include <boost/optional.hpp>

typedef Domain<unsigned long,0,15> D16 ;

// ('0',0) ('1',1) .. ('9',9) ('a',10) .. ('f',15)
static boost::optional<D16> asciiTo16(char c)
{
  D16::Type i ;
  if (('0' <= c) && (c <= '9')) {
    i = c - '0' ;
  }
  else {
    if ((c < 'a') || ('f' < c)) {
      return boost::optional<D16>() ;
    }
    i = c - 'a' + 10 ;
  }
  return D16(i) ;
}

// --------------------------------------------------------------------

std::vector<uint8_t> Format::byteV(std::string const &hex)
{
  std::vector<uint8_t> v ;
  size_t n = convert(hex.c_str(),&v) ;
  v.resize(n) ;
  convert(hex.c_str(),&v) ;
  return v ;
}

size_t Format::convert(char const hex[],Range<uint8_t> a)
{
  char const *p = hex ;
  size_t i = 0 ;
  goto Enter ;
  while (p[0] == ':') {
    ++p ;
  Enter:
    if (!p[0] || !p[1])
      throw Error("hex dyad expected") ;
    char s[3] = { p[0],p[1],0 } ;
    if (i < a.size())
      a[i] = to_ulong(s,16) ;
    ++i ;
    p += 2 ;
  }
  if (p[0] != 0)
    throw Error("hex dyads must be separated by colon (:)") ;
  return i ;
}

std::string Format::escape(std::string const &s)
{
  std::ostringstream os ;
  for (std::string::const_iterator i=s.begin() ; i!=s.end() ; ++i) {
    if ((*i) == '\\') {
      os << "\\\\" ;
    }
    else {
      int c = static_cast<unsigned char>(*i) ;
      if (0 != isprint(c)) {
	os << (*i) ;
      }
      else {
	os << "\\x" << std::setfill('0') << std::setw(2) << std::hex << c ;
      }
    }
  }
  return os.str() ;
}

std::string Format::hex(Range<uint8_t const> r)
{
  std::ostringstream os ;
  size_t i = 0 ;
  if (i < r.size()) {
    os << std::hex << std::setfill('0') << std::setw(2) << int(r[i]) ;
    while (++i < r.size()) {
      os << ":" << std::setfill('0') << std::setw(2) << int(r[i]) ;
    }
  }
  return os.str() ;
}

std::string Format::sprintf(char const format[],...)
{
  va_list args ;
  va_start(args,format) ;
  int size = vsnprintf(NULL,0,format,args) ;
  char *buffer = static_cast<char*>(malloc(size+1)) ;
  assert(buffer != NULL) ;
  va_start(args,format) ;
  int result = vsnprintf(buffer,size+1,format,args) ;
  assert(result == size) ;
  std::string s(buffer,size) ;
  free(buffer) ;
  return s ;
}

unsigned long Format::to_ulong(char const s[],size_t base)
{
  D16(base-1) ; // range check
  unsigned long v = 0 ;
  for (char const *p=s ; (*p)!=0 ; ++p) {
    boost::optional<D16> o = asciiTo16(*p) ;
    if (!o)
      throw Error("digit out of range") ;
    D16::Type d = (*o) ;
    if (d >= base)
      throw Error("digit out of range") ;
    if (std::numeric_limits<unsigned long>::max() / base < v)
      throw Error("overflow") ;
    v *= base ;
    if (std::numeric_limits<unsigned long>::max() - d < v)
      throw std::range_error("overflow") ;
    v += d ;
  }
  return v ;
}

unsigned long Format::to_ulong(char const s[])
{
  if (s[0] == '0') {
    if (s[1] == 'x') return to_ulong(s+2,16) ;
    if (s[1] == 'b') return to_ulong(s+2, 2) ;
  }
  return to_ulong(s,10) ;
}


#ifndef RENDER_MESH_HPP
#define	RENDER_MESH_HPP

#include <armadillo>
#include <vector>
#include "culling.hpp"

class Rasterizer;
class Mesh;

enum ECULLING_MODE
{
    ECM_BACKFACE = 0,
    ECM_FRONTFACE,
    ECM_NONE,

    ECULLING_MODE_COUNT
};

void draw(Rasterizer& raster, const Mesh& mesh, const arma::mat44& mat, const arma::mat44& rotmat,
        std::vector<BufferedVertice>& buff, ECULLING_MODE cull);

#endif	/* RENDER_MESH_HPP */


//
// A module to combine straw hits in the same panel.  This improves resolution and
// reduces combinatorix for downstream modules.
//  Original Author: David Brown, LBNL
//

// Mu2e includes.
#include "RecoDataProducts/inc/ComboHit.hh"
// art includes.
#include "canvas/Persistency/Common/Ptr.h"
#include "art/Framework/Core/EDProducer.h"
#include "art/Framework/Core/ModuleMacros.h"
#include "art/Framework/Principal/Event.h"
#include "art/Framework/Principal/Handle.h"

// From the art tool-chain
#include "fhiclcpp/ParameterSet.h"
#include "messagefacility/MessageLogger/MessageLogger.h"
// boost
#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/mean.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/weighted_variance.hpp>
using namespace boost::accumulators;
// C++ includes.
#include <iostream>
#include <float.h>

using namespace std;

namespace mu2e {

  class CombineStrawHits : public art::EDProducer {

  public:
    explicit CombineStrawHits(fhicl::ParameterSet const& pset);

    void produce( art::Event& e);

  private:
    // utility functions
    void combineHits(ComboHit& combohit);
    // configuration
    int _debug;
    // event object Tags
    art::InputTag _chTag;
    // input event collection cache
    const ComboHitCollection* _chcol;
    // Parameters
    bool _testflag; //test flag or not
    bool _testrad; // test position radius
    StrawHitFlag _shsel; // flag selection
    StrawHitFlag _shmask; // flag anti-selection
    float _maxdt; // maximum time separation between hits
    bool _useTOT; // use tot to estimate drift time
    float _maxwdchi; // maximum wire distance separation chi
    float _werr2; // intrinsic error on wire distance squared
    float _terr; // intrinsic error transverse to wire (per straw)
    float _minR2, _maxR2; // transverse radius (squared)
    int _maxds; // maximum straw number difference
    StrawIdMask _mask;
  };

  CombineStrawHits::CombineStrawHits(fhicl::ParameterSet const& pset) :
    art::EDProducer{pset},
    // Parameters
    _debug(pset.get<int>("debugLevel",0)),
    _chTag(pset.get<art::InputTag>("ComboHitCollection")),
    _testflag(pset.get<bool>("TestFlag")),
    _testrad(pset.get<bool>("TestRadius")),
    _shsel(pset.get<vector<string> >("StrawHitSelectionBits",vector<string>{"EnergySelection","TimeSelection"} )),
    _shmask(pset.get<vector<string> >("StrawHitMask",vector<string>{} )),
    _maxdt(pset.get<float>("MaxDt",40.0)), // nsec
    _useTOT(pset.get<bool>("UseTOT",false)), // use TOT corrected time
    _maxwdchi(pset.get<float>("MaxWireDistDiffPull",4.0)), //units of resolution sigma
    _terr(pset.get<float>("TransError",8.0)), //mm
    _maxds(pset.get<int>("MaxDS",3)), // how far away 2 straws can be, in 0-95 numbering (including layers!!)
    _mask("uniquepanel")// define the mask: ComboHits are made from straws in the same unique panel
  {
    float werr = pset.get<float>("WireError",10.0); // mm
    _werr2 = werr*werr;
    float minR = pset.get<float>("minimumRadius",395); // mm
    _minR2 = minR*minR;
    float maxR = pset.get<float>("maximumRadius",650); // mm
    _maxR2 = maxR*maxR;
    consumes<ComboHitCollection>(_chTag);
    produces<ComboHitCollection>();
  }

  void CombineStrawHits::produce(art::Event& event)
  {
    // find event data.  Note I have to get a Handle, not a ValidHandle,
    // as a literal handle is needed to find the productID
    art::Handle<ComboHitCollection> chH;
    if(!event.getByLabel(_chTag, chH))
      throw cet::exception("RECO")<<"mu2e::CombineStrawHits: No ComboHit collection found for tag" <<  _chTag << endl;
    _chcol = chH.product();

    // create output
    auto chcol = std::make_unique<ComboHitCollection>();
    chcol->reserve(_chcol->size());
    // reference the parent in the new collection
    chcol->setParent(chH);

    // sort hits by panel
    std::array<std::vector<uint16_t>,StrawId::_nupanels> panels;
    size_t nsh = _chcol->size();
    for(uint16_t ish=0;ish<nsh;++ish){
      ComboHit const& ch = (*_chcol)[ish];
      // select hits based on flag
      if((!_testflag) || (ch.flag().hasAllProperties(_shsel) && (!ch.flag().hasAnyProperty(_shmask))) ){
        panels[ch.strawId().uniquePanel()].push_back(ish);
      }
    }
    // loop over panels
    for(auto const& phits : panels ) {
      // keep track of which hits are used as part of a combo hit
      std::vector<bool> used(phits.size(),false);
      // loop over hit pairs in this panel
      for(size_t ihit=0;ihit < phits.size(); ++ihit){
        if(!used[ihit]){
          used[ihit] = true;
          ComboHit const& hit1 = (*_chcol)[phits[ihit]];
          // create a combo hit for every hit; initialize it with this hit
          ComboHit combohit;
          combohit.init(hit1,phits[ihit]);
          // loop over other hits in this panel
          for(size_t jhit=ihit+1;jhit < phits.size(); ++jhit){
            if(!used[jhit]){
              ComboHit const& hit2 = (*_chcol)[phits[jhit]];
              // require straws be near each other
              int ds = abs( (int)hit1.strawId().straw()-(int)hit2.strawId().straw());
              if(ds > 0 && ds <= _maxds ){
                // require times be consistent
                float dt;
                if (_useTOT)
		  dt = fabs(hit1.correctedTime() - hit2.correctedTime());
                else
                  dt = fabs(hit1.time() - hit2.time());
                if(dt < _maxdt){
                  // compute the chi of the differnce in wire positions
                  float wderr = sqrtf(hit1.wireErr2() + hit2.wireErr2());
                  float wdchi = fabs(hit1.wireDist() - hit2.wireDist())/wderr;
                  if(wdchi < _maxwdchi){
                    // add a neural net selection here someday for Offline use  FIXME!
                    // these hits match: add the 2nd to the combo hit
                    bool ok = combohit.addIndex(phits[jhit]);
                    if(!ok)std::cout << "CombineStrawHits past limit" << std::endl;
                    used[jhit]= true;
                  } // consistent positions along wire
                }// consistent times
              }// straw proximity
            } // 2nd hit not used
          } // 2nd panel hit
          // compute floating point info for this combo hit and save it
          if(combohit.nCombo() > 1)combineHits(combohit);
          // radius test
          float r2 = combohit.pos().Perp2();
          bool goodrad = r2 < _maxR2 && r2 > _minR2;
          if(goodrad) combohit._flag.merge(StrawHitFlag::radsel);
          if(!_testrad || goodrad)
            chcol->push_back(std::move(combohit));
        } // 1st hit not used
      } // 1st panel hit
    } // panels
    // store data in the event
    event.put(std::move(chcol));
  }

  // compute the properties of this combined hit
  void CombineStrawHits::combineHits(ComboHit& combohit) {
    // if there's only 1 hit, take the info from the orginal collections
    // This is because the boost accumulators sometimes don't work for low stats
    // init from the 0th hit
    combohit._mask = _mask;
    // add the flag
    combohit._flag.merge(StrawHitFlag::panelcombo);
    accumulator_set<float, stats<tag::mean> > eacc;
    accumulator_set<float, stats<tag::mean> > tacc;
    accumulator_set<float, stats<tag::mean> > dtacc;
    accumulator_set<float, stats<tag::mean> > ptacc;
    accumulator_set<float, stats<tag::mean> > placc;
    accumulator_set<float, stats<tag::weighted_variance(lazy)>, float> wacc;
    accumulator_set<float, stats<tag::mean> > werracc;
    XYZVec midpos;
    combohit._nsh = 0;
    if(_debug > 2)std::cout << "Combining " << combohit.nCombo() << " hits: ";
    for(size_t ich = 0; ich < combohit.nCombo(); ++ich){
      // get back the original information
      size_t index = combohit.index(ich);
      if(_debug > 3)std::cout << index << ", ";
      if(index > _chcol->size())
        throw cet::exception("RECO")<<"mu2e::CombineStrawHits: inconsistent index "<< endl;
      ComboHit const& ch = (*_chcol)[index];
      combohit._flag.merge(ch.flag());
      eacc(ch.energyDep());
      tacc(ch.time());// time is an unweighted average
      dtacc(ch.driftTime());
      ptacc(ch.propTime());
      placc(ch.pathLength());
      float wt = 1.0/(ch.wireErr2());
      wacc(ch.wireDist(),weight=wt); // wire position is weighted
      werracc(ch.wireRes());
      midpos += ch.centerPos(); // simple average for position
      combohit._nsh += ch.nStrawHits();
    }
    if(combohit.nStrawHits() < combohit.nCombo())
      throw cet::exception("RECO")<<"mu2e::CombineStrawHits: inconsistent count "<< endl;
    if(_debug > 2)std::cout << std::endl;
    combohit._time = extract_result<tag::mean>(tacc);
    combohit._dtime = extract_result<tag::mean>(dtacc);
    combohit._ptime = extract_result<tag::mean>(ptacc);
    combohit._pathlength = extract_result<tag::mean>(placc);
    combohit._edep = extract_result<tag::mean>(eacc);
    combohit._wdist = extract_result<tag::weighted_mean>(wacc);
    midpos /= combohit._nsh;
    combohit._pos = midpos + combohit._wdist*combohit._wdir;
    combohit._wres = sqrt(1.0/extract_result<tag::sum_of_weights>(wacc) + _werr2);
    combohit._tres = _terr/sqrt(combohit._nsh); // error proportional to # of straws (roughly)
    // for now, define the quality as the ratio of the variance to the average
    float wvar = sqrtf(std::max(extract_result<tag::variance>(wacc),float(0.0)));
    combohit._qual = wvar/extract_result<tag::mean>(werracc);
  }
} // end namespace mu2e

DEFINE_ART_MODULE(mu2e::CombineStrawHits)