/*
 * Representer.hxx
 *
 * Created by Remi Blanc, Marcel Luethi
 *
 * Copyright (c) 2011 ETH Zurich
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * Redistributions of source code must retain the above copyright notice,
 * this list of conditions and the following disclaimer.
 *
 * Redistributions in binary form must reproduce the above copyright
 * notice, this list of conditions and the following disclaimer in the
 * documentation and/or other materials provided with the distribution.
 *
 * Neither the name of the project's author nor the names of its
 * contributors may be used to endorse or promote products derived from
 * this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
 * TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#ifndef __ConditionalModelBuilder_hxx
#define __ConditionalModelBuilder_hxx

#include "CommonTypes.h"
#include "Exceptions.h"
#include <iostream>

#include <Eigen/SVD>
#include "PCAModelBuilder.h"
#include "ConditionalModelBuilder.h"

namespace statismo {

//
// ConditionalModelBuilder
//
//

template <typename T>
unsigned
ConditionalModelBuilder<T>::PrepareData(const DataItemListType& sampleDataList,
                                        const SurrogateTypeInfoType& surrogateTypesInfo,
                                        const CondVariableValueVectorType& conditioningInfo,
                                        DataItemListType *acceptedSamples,
                                        MatrixType *surrogateMatrix,
                                        VectorType *conditions) const {
    bool acceptSample;
    unsigned nbAcceptedSamples = 0;
    unsigned nbContinuousSurrogatesInUse = 0, nbCategoricalSurrogatesInUse = 0;
    std::vector<unsigned> indicesContinuousSurrogatesInUse;
    std::vector<unsigned> indicesCategoricalSurrogatesInUse;

    //first: identify the continuous and categorical variables, which are used for conditioning and which are not
    for (unsigned i=0 ; i<conditioningInfo.size() ; i++) {
        if (conditioningInfo[i].first) { //only variables that are used for conditioning are of interest here
            if (surrogateTypesInfo.types[i] == DataItemWithSurrogatesType::Continuous) {
                nbContinuousSurrogatesInUse++;
                indicesContinuousSurrogatesInUse.push_back(i);
            } else {
                nbCategoricalSurrogatesInUse++;
                indicesCategoricalSurrogatesInUse.push_back(i);
            }
        }
    }
    conditions->resize(nbContinuousSurrogatesInUse);
    for (unsigned i=0 ; i<nbContinuousSurrogatesInUse ; i++) (*conditions)(i) = conditioningInfo[i].second;
    surrogateMatrix->resize(nbContinuousSurrogatesInUse, sampleDataList.size()); //number of variables is now known: nbContinuousSurrogatesInUse ; the number of samples is yet unknown...

    //now, browse all samples to select the ones which fall into the requested categories
    for (typename DataItemListType::const_iterator it = sampleDataList.begin(); it != sampleDataList.end(); ++it) {
        const DataItemWithSurrogatesType* sampleData = dynamic_cast<const DataItemWithSurrogatesType*>(*it);
        if (sampleData == 0)  {
            // this is a normal sample without surrogate information.
            // we simply discard it
            std::cout<<"WARNING: ConditionalModelBuilder, sample data "<< (*it)->GetDatasetURI()<<" has no surrogate data associated, and is ignored"<<std::endl;
            continue;
        }

        VectorType surrogateData = sampleData->GetSurrogateVector();
        acceptSample = true;
        for (unsigned i=0 ; i<nbCategoricalSurrogatesInUse ; i++) { //check that this sample respect the requested categories
            if ( conditioningInfo[indicesCategoricalSurrogatesInUse[i]].second !=
                    surrogateData[indicesCategoricalSurrogatesInUse[i]] ) {
                //if one of the categories does not fit the requested one, then the sample is discarded
                acceptSample = false;
                continue;
            }
        }

        if (acceptSample) { //if the sample is of the right category
            acceptedSamples->push_back(*it);
            //and fill in the matrix of continuous variables
            for (unsigned j=0 ; j<nbContinuousSurrogatesInUse ; j++) {
                (*surrogateMatrix)(j,nbAcceptedSamples) = surrogateData[indicesContinuousSurrogatesInUse[j]];
            }
            nbAcceptedSamples++;
        }
    }
    //resize the matrix of surrogate data to the effective number of accepted samples
    surrogateMatrix->conservativeResize(Eigen::NoChange_t(), nbAcceptedSamples);

    return nbAcceptedSamples;
}

template <typename T>
typename ConditionalModelBuilder<T>::StatisticalModelType*
ConditionalModelBuilder<T>::BuildNewModel(const DataItemListType& sampleDataList,
        const SurrogateTypeInfoType& surrogateTypesInfo,
        const CondVariableValueVectorType& conditioningInfo,
        float noiseVariance,
        double modelVarianceRetained) const {
    DataItemListType acceptedSamples;
    MatrixType X;
    VectorType x0;
    unsigned nSamples = PrepareData(sampleDataList, surrogateTypesInfo, conditioningInfo, &acceptedSamples, &X, &x0);
    assert(nSamples == acceptedSamples.size());

    unsigned nCondVariables = X.rows();

    // build a normal PCA model
    typedef PCAModelBuilder<T> PCAModelBuilderType;
    PCAModelBuilderType* modelBuilder = PCAModelBuilderType::Create();
    StatisticalModelType* pcaModel = modelBuilder->BuildNewModel(acceptedSamples, noiseVariance);

    unsigned nPCAComponents = pcaModel->GetNumberOfPrincipalComponents();

    if ( X.cols() == 0 || X.rows() == 0) {
        return pcaModel;
    } else {
        // the scores in the pca model correspond to the parameters of each sample in the model.
        MatrixType B = pcaModel->GetModelInfo().GetScoresMatrix().transpose();
        assert(B.rows() == nSamples);
        assert(B.cols() == nPCAComponents);

        // A is the joint data matrix B, X, where X contains the conditional information for each sample
        // Thus the i-th row of A contains the PCA parameters b of the i-th sample,
        // together with the conditional information for each sample
        MatrixType A(nSamples, nPCAComponents+nCondVariables);
        A << B,X.transpose();

        // Compute the mean and the covariance of the joint data matrix
        VectorType mu = A.colwise().mean().transpose(); // colwise returns a row vector
        assert(mu.rows() == nPCAComponents + nCondVariables);

        MatrixType A0 = A.rowwise() - mu.transpose(); //
        MatrixType cov = 1.0 / (nSamples-1) * A0.transpose() *  A0;

        assert(cov.rows() == cov.cols());
        assert(cov.rows() == pcaModel->GetNumberOfPrincipalComponents() + nCondVariables);

        // extract the submatrices involving the conditionals x
        // note that since the matrix is symmetric, Sbx = Sxb.transpose(), hence we only store one
        MatrixType Sbx = cov.topRightCorner(nPCAComponents, nCondVariables);
        MatrixType Sxx = cov.bottomRightCorner(nCondVariables, nCondVariables);
        MatrixType Sbb = cov.topLeftCorner(nPCAComponents, nPCAComponents);

        // compute the conditional mean
        VectorType condMean = mu.topRows(nPCAComponents) + Sbx * Sxx.inverse() * (x0 - mu.bottomRows(nCondVariables));

        // compute the conditional covariance
        MatrixType condCov = Sbb - Sbx * Sxx.inverse() * Sbx.transpose();

        // get the sample mean corresponding the the conditional given mean of the parameter vectors
        VectorType condMeanSample = pcaModel->GetRepresenter()->SampleToSampleVector(pcaModel->DrawSample(condMean));

        // so far all the computation have been done in parameter (latent) space. Go back to sample space.
        // (see PartiallyFixedModelBuilder for a detailed documentation)
        // TODO we should factor this out into the base class, as it is the same code as it is used in
        // the partially fixed model builder
        const VectorType& pcaVariance = pcaModel->GetPCAVarianceVector();
        VectorTypeDoublePrecision pcaSdev = pcaVariance.cast<double>().array().sqrt();

        typedef Eigen::JacobiSVD<MatrixTypeDoublePrecision> SVDType;
        MatrixTypeDoublePrecision innerMatrix = pcaSdev.asDiagonal() * condCov.cast<double>() * pcaSdev.asDiagonal();
        SVDType svd(innerMatrix, Eigen::ComputeThinU);
        VectorType singularValues = svd.singularValues().cast<ScalarType>();

        // keep only the necessary number of modes, wrt modelVarianceRetained...
        double totalRemainingVariance = singularValues.sum(); //
        //and count the number of modes required for the model
        double cumulatedVariance = singularValues(0);
        unsigned numComponentsToReachPrescribedVariance = 1;
        while ( cumulatedVariance/totalRemainingVariance < modelVarianceRetained ) {
            numComponentsToReachPrescribedVariance++;
            if (numComponentsToReachPrescribedVariance==singularValues.size()) break;
            cumulatedVariance += singularValues(numComponentsToReachPrescribedVariance-1);
        }

        unsigned numComponentsToKeep = std::min<unsigned>( numComponentsToReachPrescribedVariance, singularValues.size() );

        VectorType newPCAVariance = singularValues.topRows(numComponentsToKeep);
        MatrixType newPCABasisMatrix = (pcaModel->GetOrthonormalPCABasisMatrix() * svd.matrixU().cast<ScalarType>()).topLeftCorner(X.cols(), numComponentsToKeep);

        StatisticalModelType* model = StatisticalModelType::Create(pcaModel->GetRepresenter(),  condMeanSample, newPCABasisMatrix, newPCAVariance, noiseVariance);

        // add builder info and data info to the info list
        MatrixType scores(0,0);
        BuilderInfo::ParameterInfoList bi;

        bi.push_back(BuilderInfo::KeyValuePair("NoiseVariance ", Utils::toString(noiseVariance)));

        //generate a matrix ; first column = boolean (yes/no, this variable is used) ; second: conditioning value.
        MatrixType conditioningInfoMatrix(conditioningInfo.size(), 2);
        for (unsigned i=0 ; i<conditioningInfo.size() ; i++) {
            conditioningInfoMatrix(i,0) = conditioningInfo[i].first;
            conditioningInfoMatrix(i,1) = conditioningInfo[i].second;
        }
        bi.push_back(BuilderInfo::KeyValuePair("ConditioningInfo ", Utils::toString(conditioningInfoMatrix)));

        typename BuilderInfo::DataInfoList di;

        unsigned i = 0;
        for (typename DataItemListType::const_iterator it = sampleDataList.begin();
                it != sampleDataList.end();
                ++it, i++) {
            const DataItemWithSurrogatesType* sampleData = dynamic_cast<const DataItemWithSurrogatesType*>(*it);
            std::ostringstream os;
            os << "URI_" << i;
            di.push_back(BuilderInfo::KeyValuePair(os.str().c_str(),sampleData->GetDatasetURI()));

            os << "_surrogates";
            di.push_back(BuilderInfo::KeyValuePair(os.str().c_str(),sampleData->GetSurrogateFilename()));
        }

        std::ostringstream os;
        os << "surrogates_types";
        di.push_back(BuilderInfo::KeyValuePair(os.str().c_str(),surrogateTypesInfo.typeFilename));

        BuilderInfo builderInfo("ConditionalModelBuilder", di, bi);

        ModelInfo::BuilderInfoList biList;
        biList.push_back(builderInfo);

        ModelInfo info(scores, biList);
        model->SetModelInfo(info);

        delete pcaModel;

        return model;
    }

}

} // namespace statismo

#endif


#include "ElNullRangeEst.h"

#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <pybind11/stl.h>

#include <Eigen/Dense>

#include <isce3/antenna/Frame.h>
#include <isce3/core/Attitude.h>
#include <isce3/core/DateTime.h>
#include <isce3/core/EMatrix.h>
#include <isce3/core/Ellipsoid.h>
#include <isce3/core/Linspace.h>
#include <isce3/core/Orbit.h>
#include <isce3/core/Poly1d.h>
#include <isce3/geometry/DEMInterpolator.h>

// Alias
namespace py = pybind11;
using namespace isce3::core;
using namespace isce3::antenna;
using isce3::geometry::DEMInterpolator;

// readonly struct or aggregate  datatypes
void addbinding(py::class_<NullProduct>& pyNullProduct)
{
    pyNullProduct.def_readonly("slant_range", &NullProduct::slant_range)
            .def_readonly("el_angle", &NullProduct::el_angle)
            .def_readonly("doppler", &NullProduct::doppler)
            .def_readonly("magnitude", &NullProduct::magnitude);
    pyNullProduct.doc() = R"(
EL null product

Attributes
----------
slant_range : float
    Slant range of the null location in (m).
el_angle : float
    Elevation angle of the null location in (rad).
doppler : float
    Doppler at the null location in (Hz).
magnitude : float
    Relative magnitude of the null w.r.t left/right peaks in (linear).
)";
}

void addbinding(py::class_<NullConvergenceFlags>& pyNullConvergenceFlags)
{
    pyNullConvergenceFlags
            .def_readonly("newton_solver", &NullConvergenceFlags::newton_solver)
            .def_readonly("geometry_echo", &NullConvergenceFlags::geometry_echo)
            .def_readonly("geometry_antenna",
                    &NullConvergenceFlags::geometry_antenna);
    pyNullConvergenceFlags.doc() = R"(
A set of flags indicating convergence of iterative operations
used in EL null product formation

Attributes
----------
newton_solver : bool
    Indicates convergence of the 1-D Newton root solver.
geometry_echo : bool
    Indicates geometry-related convergence for echo null estimation.
geometry_antenna : bool
    Indicates geometry-related convergence for antenna null estimation.
)";
}

// class
void addbinding(py::class_<ElNullRangeEst>& pyElNullRangeEst)
{
    pyElNullRangeEst
            // constructors
            .def(py::init([](double wavelength, double sr_spacing,
                                  double chirp_rate, double chirp_dur,
                                  const Orbit& orbit, const Attitude& attitude,
                                  const DEMInterpolator& dem_interp = {},
                                  const Frame& ant_frame = {},
                                  const Ellipsoid& ellips = {},
                                  double el_res = 8.726646259971648e-06,
                                  double abs_tol_dem = 1.0,
                                  int max_iter_dem = 20) {
                return ElNullRangeEst(wavelength, sr_spacing, chirp_rate,
                        chirp_dur, orbit, attitude, dem_interp, ant_frame,
                        ellips, el_res, abs_tol_dem, max_iter_dem);
            }),
                    py::arg("wavelength"), py::arg("sr_spacing"),
                    py::arg("chirp_rate"), py::arg("chirp_dur"),
                    py::arg("orbit"), py::arg("attitude"),
                    py::arg_v("dem_interp", DEMInterpolator(), "0.0"),
                    py::arg_v("ant_frame", Frame(), "EL_AND_AZ"),
                    py::arg_v("ellips", Ellipsoid(), "WGS84"),
                    py::arg("el_res") = 8.726646259971648e-06,
                    py::arg("abs_tol_dem") = 1.0, py::arg("max_iter_dem") = 20)

            // methods
            .def("genNullRangeDoppler", &ElNullRangeEst::genNullRangeDoppler,
                    py::arg("echo_left"), py::arg("echo_right"),
                    py::arg("el_cut_left"), py::arg("el_cut_right"),
                    py::arg("sr_start"), py::arg("el_ang_start"),
                    py::arg("el_ang_step"), py::arg("az_ang"),
                    py::arg("az_time") = std::nullopt, R"(
Generate null products from echo (measured) and antenna (nominal/expected).
The null product consists of azimuth time tag, null relative magnitude and its
location in EL and slant range, plut its Doppler value given
azimuth (antenna geometry)/squint(Radar geometry) angle.

Parameters
----------
echo_left : np.ndarray(complex64)
    complex 2-D array of raw echo samples (pulse by range) for
    the left RX channel corresponding to the left beam.
echo_right : np.ndarray(complex64)
    complex 2-D array of raw echo samples (pulse by range) for
    the right RX channel corresponding to the right beam.
    Must have the same shape as of that of left one!
el_cut_left : np.ndarray(complex128)
    complex array of uniformly-sampled relative or absolute
    EL-cut antenna pattern on the left side.
el_cut_right : np.ndarray(complex128)
    complex array of uniformly-sampled relative or absolute
    EL-cut antenna pattern on the right side.
    It must have the same size as left one!
sr_start : float
    start slant range (m) for both uniformly-sampled echoes in range.
el_ang_start : float
    start elevation angle for left/right EL patterns in (rad)
el_ang_step : float
    step elevation angle for left/right EL patterns in (rad)
az_ang : float
    azimuth angle (antenna geometry) or squint angle (Radar geometry)
    in (rad). This angle determines the final Doppler centroid on
    top of slant range value for both echo and antenna nulls.
az_time : float, (optional)
    azimuth time of echoes in (sec) w.r.t reference epoch of orbit.
    If not specified, the mid azimuth time of orbit will be used instead.

Returns
-------
isce3::core::DateTime
    azimuth time tag of the null product
isce3::antenna::NullProduct
    echo null product
isce3::antenna:NullProduct
    antenna null product
isce3::antenna::NullConvergenceFlags
    all flags indicating convergence of iterative operations used
    in forming both echo and antenna EL null products.

Raises
------
ValueError
    for bad input arguments
RuntimeError
    for failure in null formation

)")

            // properties
            .def_property_readonly("wave_length", &ElNullRangeEst::waveLength)
            .def_property_readonly(
                    "slant_range_spacing", &ElNullRangeEst::slantRangeSpacing)
            .def_property_readonly(
                    "grid_type_name", &ElNullRangeEst::gridTypeName)
            .def_property_readonly(
                    "chirp_sample_ref", &ElNullRangeEst::chirpSampleRef)
            .def_property_readonly("ref_epoch", &ElNullRangeEst::refEpoch)
            .def_property_readonly(
                    "dem_ref_height", &ElNullRangeEst::demRefHeight)
            .def_property_readonly(
                    "mid_time_orbit", &ElNullRangeEst::midTimeOrbit)
            .def_property_readonly(
                    "max_el_spacing", &ElNullRangeEst::maxElSpacing)
            .def_property_readonly("atol_dem", &ElNullRangeEst::atolDEM)
            .def_property_readonly("max_iter_dem", &ElNullRangeEst::maxIterDEM)
            .def_property_readonly("atol_null", &ElNullRangeEst::atolNull)
            .def_property_readonly(
                    "max_iter_null", &ElNullRangeEst::maxIterNull)
            .def_property_readonly("polyfit_deg", &ElNullRangeEst::polyfitDeg)

            .doc() = R"(
A class for forming Null power patterns in EL direction from both
a pair of adjacent El-cut antenna patterns as well as the respective
raw echoes of two adjacent RX channels.
The location of null in both antenna and echo domain will be estimated
and their respective values in EL angle, slant range, and Doppler will
be reported at a specific azimuth time in orbit.
See the following link and its references for algorithm, simulation and analyses,
https://github.jpl.nasa.gov/NISAR-POINTING/DOC/blob/master/Null_ELPointEst_REEsim_RevB.pdf
)";
}


#include <cctbx/boost_python/flex_fwd.h>

//#include <cctbx/miller/sym_equiv.h>
//#include <cctbx/miller/union_of_indices.h>
#include <cctbx/miller/math.h>
#include <boost/python.hpp>
#include <scitbx/boost_python/container_conversions.h>

namespace yamtbx { namespace dataproc { namespace crystfel { namespace boost_python {
  void wrap_merge_equivalents();

namespace {
  void init_module()
  {
    using namespace boost::python;

    wrap_merge_equivalents();
  }
} // namespace <anonymous>
}}}} // namespace yamtbx::dataproc::crystfel::boost_python

BOOST_PYTHON_MODULE(yamtbx_dataproc_crystfel_ext)
{
  yamtbx::dataproc::crystfel::boost_python::init_module();
}


#include <Eigen/Dense>
#include <Eigen/Sparse>

#include <Data/Shape.h>

#include <QFile>
#include <QFileInfo>
#include <QMessageBox>
#include <QTextStream>

#include <itkOrientImageFilter.h>

#include <vtkCenterOfMass.h>

#include <Data/MeshGenerator.h>
#include <Data/StudioLog.h>
#include <Visualization/Visualizer.h>
#include <Libs/Project/ProjectUtils.h>

using ReaderType = itk::ImageFileReader<ImageType>;

namespace shapeworks {

//---------------------------------------------------------------------------
Shape::Shape()
{
  this->id_ = 0;

  this->corner_annotations_ << "";
  this->corner_annotations_ << "";
  this->corner_annotations_ << "";
  this->corner_annotations_ << "";
}

//---------------------------------------------------------------------------
Shape::~Shape() = default;

//---------------------------------------------------------------------------
MeshGroup Shape::get_meshes(const string& display_mode)
{
  if (display_mode == Visualizer::MODE_ORIGINAL_C) {
    return this->get_original_meshes();
  }
  else if (display_mode == Visualizer::MODE_GROOMED_C) {
    return this->get_groomed_meshes();
  }
  return this->get_reconstructed_meshes();
}

//---------------------------------------------------------------------------
void Shape::set_annotations(QStringList annotations, bool only_overwrite_blank)
{
  if (only_overwrite_blank && !this->corner_annotations_.empty() &&
      this->corner_annotations_[0] != "") {
    return; // don't override
  }
  this->corner_annotations_ = annotations;
}

//---------------------------------------------------------------------------
QStringList Shape::get_annotations()
{
  return this->corner_annotations_;
}

//---------------------------------------------------------------------------
void Shape::set_mesh_manager(QSharedPointer<MeshManager> mesh_manager)
{
  this->mesh_manager_ = mesh_manager;
}

//---------------------------------------------------------------------------
void Shape::set_subject(std::shared_ptr<Subject> subject)
{
  this->subject_ = subject;
  this->original_meshes_.set_number_of_meshes(subject->get_number_of_domains());
  this->groomed_meshes_.set_number_of_meshes(subject->get_number_of_domains());
  this->reconstructed_meshes_.set_number_of_meshes(subject->get_number_of_domains());

  if (!this->subject_->get_segmentation_filenames().empty()) {

    /// TODO: Show multiple lines of filenames for multiple domains?
    std::string filename = this->subject_->get_segmentation_filenames()[0];
    this->corner_annotations_[0] = QFileInfo(QString::fromStdString(filename)).fileName();
  }

  if (subject->get_display_name() != "") {
    this->corner_annotations_[0] = QString::fromStdString(subject->get_display_name());
  }
}

//---------------------------------------------------------------------------
std::shared_ptr<Subject> Shape::get_subject()
{
  return this->subject_;
}

//---------------------------------------------------------------------------
void Shape::import_original_image(const std::string& filename)
{
  this->subject_->set_segmentation_filenames(std::vector<std::string>{filename});
  this->corner_annotations_[0] = QFileInfo(QString::fromStdString(filename)).fileName();
}

//---------------------------------------------------------------------------
MeshGroup Shape::get_original_meshes(bool wait)
{
  if (!this->subject_) {
    std::cerr << "Error: asked for original mesh when none is present!\n";
    assert(0);
  }

  if (!this->original_meshes_.valid()) {
    this->generate_meshes(this->subject_->get_segmentation_filenames(), this->original_meshes_,
                          true, wait);
  }
  return this->original_meshes_;
}

//---------------------------------------------------------------------------
MeshGroup Shape::get_groomed_meshes(bool wait)
{
  if (!this->subject_) {
    std::cerr << "Error: asked for groomed meshes when none are present!\n";
    assert(0);
  }

  if (!this->groomed_meshes_.valid()) {
    this->generate_meshes(this->subject_->get_groomed_filenames(), this->groomed_meshes_,
                          true, wait);
  }
  return this->groomed_meshes_;
}

//---------------------------------------------------------------------------
MeshGroup Shape::get_reconstructed_meshes(bool wait)
{
  if (!this->reconstructed_meshes_.valid()) {
    auto worlds = this->particles_.get_world_particles();
    this->reconstructed_meshes_.set_number_of_meshes(worlds.size());
    for (int i = 0; i < worlds.size(); i++) {
      MeshHandle mesh = this->mesh_manager_->get_mesh(worlds[i], i);
      if (mesh) {
        this->reconstructed_meshes_.set_mesh(i, mesh);
      }
    }
  }
  return this->reconstructed_meshes_;
}

//---------------------------------------------------------------------------
void Shape::reset_groomed_mesh()
{
  this->groomed_meshes_ = MeshGroup(this->subject_->get_number_of_domains());
}

//---------------------------------------------------------------------------
void Shape::clear_reconstructed_mesh()
{
  this->reconstructed_meshes_ = MeshGroup(this->subject_->get_number_of_domains());
}

//---------------------------------------------------------------------------
bool Shape::import_global_point_files(QStringList filenames)
{
  for (int i = 0; i < filenames.size(); i++) {
    Eigen::VectorXd points;
    if (!Shape::import_point_file(filenames[i], points)) {
      return false;
    }
    this->global_point_filenames_.push_back(filenames[i].toStdString());
    this->particles_.set_world_particles(i, points);
  }
  this->subject_->set_world_particle_filenames(this->global_point_filenames_);
  return true;
}

//---------------------------------------------------------------------------
bool Shape::import_local_point_files(QStringList filenames)
{
  for (int i = 0; i < filenames.size(); i++) {
    Eigen::VectorXd points;
    if (!Shape::import_point_file(filenames[i], points)) {
      throw std::invalid_argument("Unable to load file: " + filenames[i].toStdString());
    }
    this->local_point_filenames_.push_back(filenames[i].toStdString());
    this->particles_.set_local_particles(i, points);
  }
  this->subject_->set_local_particle_filenames(this->local_point_filenames_);
  return true;
}

//---------------------------------------------------------------------------
Eigen::VectorXd Shape::get_global_correspondence_points()
{
  return this->particles_.get_combined_global_particles();
}

//---------------------------------------------------------------------------
Eigen::VectorXd Shape::get_local_correspondence_points()
{
  return this->particles_.get_combined_local_particles();
}

//---------------------------------------------------------------------------
int Shape::get_id()
{
  return this->id_;
}

//---------------------------------------------------------------------------
void Shape::set_id(int id)
{
  this->id_ = id;
}

//---------------------------------------------------------------------------
QString Shape::get_original_filename()
{
  if (this->subject_->get_segmentation_filenames().size() < 1) {
    return "";
  }
  auto string = QString::fromStdString(this->subject_->get_segmentation_filenames()[0]);
  QFileInfo info(string);
  return info.fileName();
}

//---------------------------------------------------------------------------
QString Shape::get_original_filename_with_path()
{
  if (this->subject_->get_segmentation_filenames().size() < 1) {
    return "";
  }
  return QString::fromStdString(this->subject_->get_segmentation_filenames()[0]);
}

//---------------------------------------------------------------------------
std::vector<QString> Shape::get_original_filenames()
{
  if (this->subject_->get_segmentation_filenames().size() < 1) {
    return std::vector<QString>();
  }

  std::vector<QString> filenames;
  for (auto&& name : this->subject_->get_segmentation_filenames()) {
    filenames.push_back(QFileInfo(QString::fromStdString(name)).fileName());
  }

  return filenames;
}

//---------------------------------------------------------------------------
std::vector<QString> Shape::get_original_filenames_with_path()
{
  if (this->subject_->get_segmentation_filenames().size() < 1) {
    return std::vector<QString>();
  }

  std::vector<QString> filenames;
  for (auto&& name : this->subject_->get_segmentation_filenames()) {
    filenames.push_back(QString::fromStdString(name));
  }

  return filenames;
}

//---------------------------------------------------------------------------
QString Shape::get_groomed_filename()
{
  if (this->subject_->get_groomed_filenames().size() < 1) {
    return "";
  }
  auto string = QString::fromStdString(this->subject_->get_groomed_filenames()[0]);
  QFileInfo info(string);
  return info.fileName();
}

//---------------------------------------------------------------------------
QString Shape::get_groomed_filename_with_path(int domain)
{
  if (domain >= this->subject_->get_groomed_filenames().size()) {
    return "";
  }
  return QString::fromStdString(this->subject_->get_groomed_filenames()[domain]);
}

//---------------------------------------------------------------------------
QList<Shape::Point> Shape::get_exclusion_sphere_centers()
{
  return this->exclusion_sphere_centers_;
}

//---------------------------------------------------------------------------
void Shape::set_exclusion_sphere_centers(QList<Shape::Point> centers)
{
  this->exclusion_sphere_centers_ = centers;
}

//---------------------------------------------------------------------------
QList<double> Shape::get_exclusion_sphere_radii()
{
  return this->exclusion_sphere_radii_;
}

//---------------------------------------------------------------------------
void Shape::set_exclusion_sphere_radii(QList<double> radii)
{
  this->exclusion_sphere_radii_ = radii;
}

//---------------------------------------------------------------------------
int Shape::get_group_id()
{
  return this->group_id_;
}

//---------------------------------------------------------------------------
void Shape::set_group_id(int id)
{
  if (this->subject_) {
    this->subject_->set_group_values({{"group", std::to_string(id)}});
  }
  this->group_id_ = id;
}

//---------------------------------------------------------------------------
std::vector<Shape::Point> Shape::get_vectors()
{
  return this->vectors_;
}

//---------------------------------------------------------------------------
void Shape::set_vectors(std::vector<Shape::Point> vectors)
{
  this->vectors_ = vectors;
}

//---------------------------------------------------------------------------
void Shape::set_transform(vtkSmartPointer<vtkTransform> transform)
{
  this->transform_ = transform;
}

//---------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> Shape::get_transform(int domain)
{
  auto groom_transform = this->get_groomed_transform(domain);
  if (!groom_transform) {
    return this->transform_;
  }
  return groom_transform;
}

//---------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> Shape::get_alignment(int domain)
{
  auto groom_transform = this->get_groomed_transform(domain);
  if (!groom_transform) {
    vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
    transform->Identity();
    return transform;
  }
  return groom_transform;
}

//---------------------------------------------------------------------------
bool Shape::has_alignment()
{
  auto groom_transform = this->get_groomed_transform(0);
  if (groom_transform) {
    return true;
  }

  return false;
}

//---------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> Shape::get_original_transform(int domain)
{
  return this->transform_;
}

//---------------------------------------------------------------------------
void Shape::generate_meshes(std::vector<std::string> filenames, MeshGroup& mesh_group,
                            bool save_transform, bool wait)
{
  if (filenames.empty()) {
    return;
  }

  for (int i = 0; i < filenames.size(); i++) {
    auto filename = filenames[i];
    MeshWorkItem item;
    item.filename = filename;
    MeshHandle new_mesh = this->mesh_manager_->get_mesh(item, wait);
    if (new_mesh) {
      mesh_group.set_mesh(i, new_mesh);

      // generate a basic centering transform
      auto com = vtkSmartPointer<vtkCenterOfMass>::New();
      com->SetInputData(new_mesh->get_poly_data());
      com->Update();
      double center[3];
      com->GetCenter(center);

      if (save_transform && i == 0) { // only store for first domain
        this->transform_->Identity();
        this->transform_->Translate(-center[0], -center[1], -center[2]);
      }
    }
  }
}

//---------------------------------------------------------------------------
bool Shape::import_point_file(QString filename, Eigen::VectorXd& points)
{
  std::ifstream in(filename.toStdString().c_str());
  if (!in.good()) {
    return false;
  }
  vtkSmartPointer<vtkPoints> vtk_points = vtkSmartPointer<vtkPoints>::New();
  auto test = filename.toStdString();
  int num_points = 0;
  while (in.good()) {
    double x, y, z;
    in >> x >> y >> z;
    if (!in.good()) { break; }
    vtk_points->InsertNextPoint(x, y, z);
    num_points++;
  }
  in.close();
  points.setZero();
  points.resize(num_points * 3);

  int idx = 0;
  for (int i = 0; i < num_points; i++) {
    double* pos = vtk_points->GetPoint(i);
    points[idx++] = pos[0];
    points[idx++] = pos[1];
    points[idx++] = pos[2];
  }
  return true;
}

//---------------------------------------------------------------------------
void Shape::load_feature(std::string display_mode, std::string feature)
{
  auto group = this->get_meshes(display_mode);
  if (!group.valid()) {
    // not ready yet
    return;
  }

  int num_domains = group.meshes().size();

  for (int d = 0; d < num_domains; d++) {

    vtkSmartPointer<vtkPolyData> poly_data = group.meshes()[d]->get_poly_data();

    // first check if we already have this array
    auto scalar_array = poly_data->GetPointData()->GetArray(feature.c_str());
    if (!scalar_array) {

      if (!this->subject_) {
        return;
      }

      // first check if we have particle scalars for this feature
      auto point_features = this->get_point_features(feature);
      if (point_features.size() > 0 && display_mode == Visualizer::MODE_RECONSTRUCTION_C) { // already loaded as particle scalars
        this->set_point_features(feature, point_features);
      }
      else {
        // next check if there is a feature filename
        auto filenames = this->subject_->get_feature_filenames();
        if (filenames.find(feature) == filenames.end()) {
          // no feature filename, so load it from the original mesh
          auto original_meshes = this->get_original_meshes(true).meshes();

          if (original_meshes.size() > d) {
            // assign scalars at points
            this->load_feature_from_mesh(feature, original_meshes[d]);
          }
        }
        else {
          // read the feature
          QString filename = QString::fromStdString(filenames[feature]);
          try {
            ReaderType::Pointer reader = ReaderType::New();
            reader->SetFileName(filename.toStdString());
            reader->Update();
            ImageType::Pointer image = reader->GetOutput();
            group.meshes()[d]->apply_feature_map(feature, image);
            this->apply_feature_to_points(feature, image);
          } catch (itk::ExceptionObject& excep) {
            QMessageBox::warning(0, "Unable to open file",
                                 "Error opening file: \"" + filename + "\"");
          }
        }
      }
    }
  }
}

//---------------------------------------------------------------------------
void Shape::apply_feature_to_points(std::string feature, ImageType::Pointer image)
{
  using LinearInterpolatorType = itk::LinearInterpolateImageFunction<ImageType, double>;

  LinearInterpolatorType::Pointer interpolator = LinearInterpolatorType::New();
  interpolator->SetInputImage(image);

  auto region = image->GetLargestPossibleRegion();

  Eigen::VectorXd all_locals = this->get_local_correspondence_points();

  int num_points = all_locals.size() / 3;

  Eigen::VectorXf values(num_points);

  int idx = 0;
  for (int i = 0; i < num_points; ++i) {

    double p[3];
    p[0] = all_locals[idx++];
    p[1] = all_locals[idx++];
    p[2] = all_locals[idx++];

    double* pt = p;

    ImageType::PointType pitk;
    pitk[0] = pt[0];
    pitk[1] = pt[1];
    pitk[2] = pt[2];

    LinearInterpolatorType::ContinuousIndexType index;
    image->TransformPhysicalPointToContinuousIndex(pitk, index);

    auto pixel = 0;
    if (region.IsInside(index)) {
      pixel = interpolator->EvaluateAtContinuousIndex(index);
    }

    values[i] = pixel;
  }

  this->set_point_features(feature, values);
}

//---------------------------------------------------------------------------
void Shape::load_feature_from_mesh(std::string feature, MeshHandle mesh)
{
  vtkSmartPointer<vtkPolyData> from_mesh = mesh->get_poly_data();

  // Create the tree
  vtkSmartPointer<vtkKdTreePointLocator> kd_tree = vtkSmartPointer<vtkKdTreePointLocator>::New();
  kd_tree->SetDataSet(from_mesh);
  kd_tree->BuildLocator();

  Eigen::VectorXd all_locals = this->get_local_correspondence_points();

  int num_points = all_locals.size() / 3;

  Eigen::VectorXf values(num_points);

  vtkDataArray* from_array = from_mesh->GetPointData()->GetArray(feature.c_str());
  if (!from_array) {
    return;
  }

  int idx = 0;
  for (int i = 0; i < num_points; ++i) {

    double p[3];
    p[0] = all_locals[idx++];
    p[1] = all_locals[idx++];
    p[2] = all_locals[idx++];

    vtkIdType id = kd_tree->FindClosestPoint(p);
    vtkVariant var = from_array->GetVariantValue(id);

    values[i] = var.ToDouble();
  }

  this->set_point_features(feature, values);
}

//---------------------------------------------------------------------------
Eigen::VectorXf Shape::get_point_features(std::string feature)
{
  auto it = this->point_features_.find(feature);
  if (it == this->point_features_.end()) {
    return Eigen::VectorXf();
  }

  return it->second;
}

//---------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> Shape::get_groomed_transform(int domain)
{
  auto transforms = this->subject_->get_groomed_transforms();
  if (domain < 0) { // global alignment is stored at the end
    domain = transforms.size() - 1;
  }
  if (domain < transforms.size()) {
    return ProjectUtils::convert_transform(transforms[domain]);
  }
  return nullptr;
}

//---------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> Shape::get_procrustest_transform(int domain)
{
  auto transforms = this->subject_->get_procrustes_transforms();
  if (domain < transforms.size()) {
    return ProjectUtils::convert_transform(transforms[domain]);
  }
  return nullptr;
}

//---------------------------------------------------------------------------
std::vector<vtkSmartPointer<vtkTransform>> Shape::get_procrustest_transforms()
{
  auto lists = this->subject_->get_procrustes_transforms();
  std::vector<vtkSmartPointer<vtkTransform>> transforms;
  for (size_t i = 0; i < lists.size(); i++) {
    transforms.push_back(ProjectUtils::convert_transform(lists[i]));
  }
  return transforms;
}

//---------------------------------------------------------------------------
void Shape::set_point_features(std::string feature, Eigen::VectorXf values)
{
  this->point_features_[feature] = values;

  auto group = this->get_meshes(Visualizer::MODE_RECONSTRUCTION_C);

  if (group.valid()) {
    for (auto mesh : group.meshes()) {
      mesh->interpolate_scalars_to_mesh(feature,
                                        this->get_global_correspondence_points(), values);
    }
  }
}

//---------------------------------------------------------------------------
void Shape::set_particles(StudioParticles particles)
{
  this->particles_ = particles;
}

//---------------------------------------------------------------------------
StudioParticles Shape::get_particles()
{
  return this->particles_;
}

//---------------------------------------------------------------------------
void Shape::set_particle_transform(vtkSmartPointer<vtkTransform> transform)
{
  this->particles_.set_procrustes_transforms(this->get_procrustest_transforms());
  this->particles_.set_transform(transform);
}

//---------------------------------------------------------------------------
vtkSmartPointer<vtkTransform> Shape::get_reconstruction_transform(int domain)
{
  if (domain < this->reconstruction_transforms_.size()) {
    return this->reconstruction_transforms_[domain];
  }

  // no transforms, just return identity
  vtkSmartPointer<vtkTransform> transform = vtkSmartPointer<vtkTransform>::New();
  transform->Identity();
  return transform;
}

//---------------------------------------------------------------------------
Eigen::VectorXd Shape::get_global_correspondence_points_for_display()
{
  auto worlds = this->particles_.get_world_particles();
  int size = 0;
  for (int i = 0; i < worlds.size(); i++) {
    size += worlds[i].size();
  }
  Eigen::VectorXd points;
  points.resize(size);

  int idx = 0;
  for (int i = 0; i < worlds.size(); i++) {
    for (int j = 0; j < worlds[i].size(); j += 3) {
      double p[3];
      p[0] = worlds[i][j + 0];
      p[1] = worlds[i][j + 1];
      p[2] = worlds[i][j + 2];
      if (this->reconstruction_transforms_.size() > i) {
        double* pt = this->reconstruction_transforms_[i]->TransformPoint(p);
        points[idx++] = pt[0];
        points[idx++] = pt[1];
        points[idx++] = pt[2];
      }
      else {
        points[idx++] = p[0];
        points[idx++] = p[1];
        points[idx++] = p[2];
      }
    }
  }

  return points;
}

//---------------------------------------------------------------------------
void Shape::set_reconstruction_transforms(std::vector<vtkSmartPointer<vtkTransform>> transforms)
{
  this->reconstruction_transforms_ = transforms;
}

//---------------------------------------------------------------------------
void Shape::load_feature_from_scalar_file(std::string filename, std::string feature_name)
{
  QString qfilename = QString::fromStdString(filename);

  if (!QFile(qfilename).exists()) {
    return;
  }

  QFile file(qfilename);
  if (!file.open(QIODevice::ReadOnly)) {
    STUDIO_LOG_ERROR("Unable to open scalar file: " + qfilename);
    return;
  }

  auto data = QString(file.readAll()).trimmed();
  auto lines = data.split('\n');
  file.close();

  Eigen::VectorXf values(lines.size());
  for (int i = 0; i < lines.size(); i++) {
    float value = QString(lines[i]).toFloat();
    values[i] = value;
  }

  this->set_point_features(feature_name, values);
}

//---------------------------------------------------------------------------
void Shape::set_override_feature(string feature)
{
  this->override_feature_ = feature;
}

//---------------------------------------------------------------------------
string Shape::get_override_feature()
{
  return this->override_feature_;
}
}


// Copyright (c) 2015-2018 Daniel Cooke
// Use of this source code is governed by the MIT license that can be found in the LICENSE file.

#include "cell_caller.hpp"

#include <typeinfo>
#include <unordered_map>
#include <deque>
#include <algorithm>
#include <numeric>
#include <iterator>
#include <utility>
#include <stdexcept>
#include <iostream>

#include <boost/iterator/zip_iterator.hpp>
#include <boost/tuple/tuple.hpp>

#include "basics/genomic_region.hpp"
#include "containers/probability_matrix.hpp"
#include "core/types/allele.hpp"
#include "core/types/variant.hpp"
#include "core/types/phylogeny.hpp"
#include "core/types/calls/cell_variant_call.hpp"
#include "core/types/calls/reference_call.hpp"
#include "core/models/genotype/uniform_genotype_prior_model.hpp"
#include "core/models/genotype/coalescent_genotype_prior_model.hpp"
#include "core/models/mutation/denovo_model.hpp"
#include "core/models/genotype/single_cell_prior_model.hpp"
#include "utils/maths.hpp"
#include "logging/logging.hpp"

namespace octopus {

CellCaller::CellCaller(Caller::Components&& components,
                       Caller::Parameters general_parameters,
                       Parameters specific_parameters)
: Caller {std::move(components), std::move(general_parameters)}
, parameters_{std::move(specific_parameters)}
{}

std::string CellCaller::do_name() const
{
    return "cell";
}

CellCaller::CallTypeSet CellCaller::do_call_types() const
{
    return {std::type_index(typeid(CellVariantCall))};
}

unsigned CellCaller::do_min_callable_ploidy() const
{
    return parameters_.ploidy;
}

unsigned CellCaller::do_max_callable_ploidy() const
{
    return parameters_.ploidy;
}

std::size_t CellCaller::do_remove_duplicates(std::vector<Haplotype>& haplotypes) const
{
    if (parameters_.deduplicate_haplotypes_with_prior_model) {
        if (haplotypes.size() < 2) return 0;
        CoalescentModel::Parameters model_params {};
        if (parameters_.prior_model_params) model_params = *parameters_.prior_model_params;
        Haplotype reference {mapped_region(haplotypes.front()), reference_.get()};
        CoalescentModel model {std::move(reference), model_params, haplotypes.size(), CoalescentModel::CachingStrategy::none};
        const CoalescentProbabilityGreater cmp {std::move(model)};
        return octopus::remove_duplicates(haplotypes, cmp);
    } else {
        return Caller::do_remove_duplicates(haplotypes);
    }
}

// CellCaller::Latents public methods

CellCaller::Latents::Latents(const CellCaller& caller,
                             std::vector<Haplotype> haplotypes,
                             std::vector<Genotype<Haplotype>> genotypes,
                             std::vector<model::SingleCellModel::Inferences> inferences)
: caller_ {caller}
, haplotypes_ {std::move(haplotypes)}
, genotypes_ {std::move(genotypes)}
, phylogeny_inferences_ {std::move(inferences)}
{
    phylogeny_posteriors_.resize(phylogeny_inferences_.size());
    std::transform(std::cbegin(phylogeny_inferences_), std::cend(phylogeny_inferences_), std::begin(phylogeny_posteriors_),
                   [] (const auto& inferences) { return inferences.log_evidence; });
    maths::normalise_exp(phylogeny_posteriors_);
}

namespace {

using InverseGenotypeTable = std::vector<std::vector<std::size_t>>;

auto make_inverse_genotype_table(const std::vector<Haplotype>& haplotypes,
                                 const std::vector<Genotype<Haplotype>>& genotypes)
{
    assert(!haplotypes.empty() && !genotypes.empty());
    using HaplotypeReference = std::reference_wrapper<const Haplotype>;
    std::unordered_map<HaplotypeReference, std::vector<std::size_t>> result_map {haplotypes.size()};
    const auto cardinality = element_cardinality_in_genotypes(static_cast<unsigned>(haplotypes.size()),
                                                              genotypes.front().ploidy());
    for (const auto& haplotype : haplotypes) {
        auto itr = result_map.emplace(std::piecewise_construct,
                                      std::forward_as_tuple(std::cref(haplotype)),
                                      std::forward_as_tuple());
        itr.first->second.reserve(cardinality);
    }
    for (std::size_t i {0}; i < genotypes.size(); ++i) {
        for (const auto& haplotype : genotypes[i]) {
            result_map.at(haplotype).emplace_back(i);
        }
    }
    InverseGenotypeTable result {};
    result.reserve(haplotypes.size());
    for (const auto& haplotype : haplotypes) {
        auto& indices = result_map.at(haplotype);
        std::sort(std::begin(indices), std::end(indices));
        indices.erase(std::unique(std::begin(indices), std::end(indices)), std::end(indices));
        result.emplace_back(std::move(indices));
    }
    return result;
}

using GenotypeMarginalPosteriorVector = std::vector<double>;
using GenotypeMarginalPosteriorMatrix = std::vector<GenotypeMarginalPosteriorVector>;

auto calculate_haplotype_posteriors(const std::vector<Haplotype>& haplotypes,
                                    const std::vector<Genotype<Haplotype>>& genotypes,
                                    const ProbabilityMatrix<Genotype<Haplotype>>& genotype_posteriors,
                                    const InverseGenotypeTable& inverse_genotypes)
{
    std::unordered_map<std::reference_wrapper<const Haplotype>, double> result {haplotypes.size()};
    auto itr = std::cbegin(inverse_genotypes);
    std::vector<std::size_t> genotype_indices(genotypes.size());
    std::iota(std::begin(genotype_indices), std::end(genotype_indices), 0);
    // noncontaining genotypes are genotypes that do not contain a particular haplotype.
    const auto num_noncontaining_genotypes = genotypes.size() - itr->size();
    std::vector<std::size_t> noncontaining_genotype_indices(num_noncontaining_genotypes);
    for (const auto& haplotype : haplotypes) {
        std::set_difference(std::cbegin(genotype_indices), std::cend(genotype_indices),
                            std::cbegin(*itr), std::cend(*itr),
                            std::begin(noncontaining_genotype_indices));
        double prob_not_observed {1};
        for (const auto& p : genotype_posteriors) {
            const auto slice = genotype_posteriors(p.first);
            std::vector<double> sample_genotype_posteriors {std::cbegin(slice), std::cend(slice)};
            prob_not_observed *= std::accumulate(std::cbegin(noncontaining_genotype_indices),
                                                 std::cend(noncontaining_genotype_indices),
                                                 0.0, [&sample_genotype_posteriors]
                                                 (const auto curr, const auto i) {
                return curr + sample_genotype_posteriors[i];
            });
        }
        result.emplace(haplotype, 1.0 - prob_not_observed);
        ++itr;
    }
    return result;
}

auto calculate_haplotype_posteriors(const std::vector<Haplotype>& haplotypes,
                                    const std::vector<Genotype<Haplotype>>& genotypes,
                                    const ProbabilityMatrix<Genotype<Haplotype>>& genotype_posteriors)
{
    const auto inverse_genotypes = make_inverse_genotype_table(haplotypes, genotypes);
    return calculate_haplotype_posteriors(haplotypes, genotypes, genotype_posteriors, inverse_genotypes);
}

} // namespace

std::shared_ptr<CellCaller::Latents::HaplotypeProbabilityMap>
CellCaller::Latents::haplotype_posteriors() const noexcept
{
    if (haplotype_posteriors_ == nullptr) {
        const auto marginal_genotype_posteriors = this->genotype_posteriors();
        haplotype_posteriors_ = std::make_unique<HaplotypeProbabilityMap>(calculate_haplotype_posteriors(haplotypes_, genotypes_, *marginal_genotype_posteriors));
    }
    return haplotype_posteriors_;
}

namespace {

template <typename... T>
auto zip(const T&... containers) -> boost::iterator_range<boost::zip_iterator<decltype(boost::make_tuple(std::begin(containers)...))>>
{
    auto zip_begin = boost::make_zip_iterator(boost::make_tuple(std::begin(containers)...));
    auto zip_end   = boost::make_zip_iterator(boost::make_tuple(std::end(containers)...));
    return boost::make_iterator_range(zip_begin, zip_end);
}

} // namespace

std::shared_ptr<CellCaller::Latents::GenotypeProbabilityMap>
CellCaller::Latents::genotype_posteriors() const noexcept
{
    if (genotype_posteriors_ == nullptr) {
        genotype_posteriors_ = std::make_unique<GenotypeProbabilityMap>(std::cbegin(genotypes_), std::cend(genotypes_));
        for (std::size_t sample_idx {0}; sample_idx < caller_.samples_.size(); ++sample_idx) {
            std::vector<double> marginal_genotype_posteriors(genotypes_.size());
            for (const auto& p : zip(phylogeny_inferences_, phylogeny_posteriors_)) {
                const auto& phylogeny = p.get<0>().phylogeny;
                for (unsigned t {0}; t < phylogeny.size(); ++t) {
                    const auto& group = phylogeny.group(t).value;
                    for (std::size_t genotype_idx {0}; genotype_idx < group.genotype_posteriors.size(); ++genotype_idx) {
                        marginal_genotype_posteriors[genotype_idx] += p.get<1>()
                                * group.sample_attachment_posteriors[sample_idx]
                                * group.genotype_posteriors[genotype_idx];
                    }
                }
            }
            insert_sample(caller_.samples_[sample_idx], std::move(marginal_genotype_posteriors), *genotype_posteriors_);
        }
    }
    return genotype_posteriors_;
}

// CellCaller::Latents private methods

template <typename S>
void log(const model::SingleCellModel::Inferences& inferences,
         const std::vector<SampleName>& samples,
         const std::vector<Genotype<Haplotype>>& genotypes,
         S&& logger)
{
    std::vector<std::size_t> map_genotypes {};
    map_genotypes.reserve(inferences.phylogeny.size());
    std::vector<std::pair<std::size_t, double>> map_sample_assignments(samples.size());
    for (std::size_t group_id {0}; group_id < inferences.phylogeny.size(); ++group_id) {
        const auto& group = inferences.phylogeny.group(group_id).value;
        auto map_itr = std::max_element(std::cbegin(group.genotype_posteriors), std::cend(group.genotype_posteriors));
        auto map_idx = static_cast<std::size_t>(std::distance(std::cbegin(group.genotype_posteriors), map_itr));
        map_genotypes.push_back(map_idx);
        for (std::size_t sample_idx {0}; sample_idx < samples.size(); ++sample_idx) {
            if (group.sample_attachment_posteriors[sample_idx] > map_sample_assignments[sample_idx].second) {
                map_sample_assignments[sample_idx].first = group_id;
                map_sample_assignments[sample_idx].second = group.sample_attachment_posteriors[sample_idx];
            }
        }
    }
    logger << "MAP genotypes: " << '\n';
    for (std::size_t group_id {0}; group_id < map_genotypes.size(); ++group_id) {
        logger << group_id << ": "; debug::print_variant_alleles(logger, genotypes[map_genotypes[group_id]]); logger << '\n';
    }
    logger << "Sample MAP assignments:" << '\n';
    for (std::size_t sample_idx {0}; sample_idx < samples.size(); ++sample_idx) {
        logger << samples[sample_idx] << ": " << map_sample_assignments[sample_idx].first
               << " (" << map_sample_assignments[sample_idx].second << ")\n";
    }
    logger << "Evidence: " << inferences.log_evidence << '\n';
}

void log(const model::SingleCellModel::Inferences& inferences,
         const std::vector<SampleName>& samples,
         const std::vector<Genotype<Haplotype>>& genotypes,
         boost::optional<logging::DebugLogger>& logger)
{
    if (logger) {
        log(inferences, samples, genotypes, stream(*logger));
    }
}

std::unique_ptr<CellCaller::Caller::Latents>
CellCaller::infer_latents(const std::vector<Haplotype>& haplotypes, const HaplotypeLikelihoodArray& haplotype_likelihoods) const
{
    std::vector<GenotypeIndex> genotype_indices {};
    auto genotypes = generate_all_genotypes(haplotypes, parameters_.ploidy, genotype_indices);
    if (debug_log_) stream(*debug_log_) << "There are " << genotypes.size() << " candidate genotypes";
    const auto genotype_prior_model = make_prior_model(haplotypes);
    DeNovoModel mutation_model {parameters_.mutation_model_parameters};
    model::SingleCellPriorModel::Parameters cell_prior_params {};
    cell_prior_params.copy_number_log_probability = std::log(1e-6);
    model::SingleCellModel::Parameters model_parameters {};
    model_parameters.dropout_concentration = parameters_.dropout_concentration;
    model_parameters.group_concentration = 1.0;
    model::SingleCellModel::AlgorithmParameters config {};
    config.max_genotype_combinations = parameters_.max_joint_genotypes;
    if (parameters_.max_vb_seeds) config.max_seeds = *parameters_.max_vb_seeds;

    using CellPhylogeny =  model::SingleCellPriorModel::CellPhylogeny;
    CellPhylogeny single_group_phylogeny {CellPhylogeny::Group {0}};
    model::SingleCellPriorModel single_group_prior_model {std::move(single_group_phylogeny), *genotype_prior_model, mutation_model, cell_prior_params};
    model::SingleCellModel single_group_model {samples_, std::move(single_group_prior_model), model_parameters, config};
    auto single_group_inferences = single_group_model.evaluate(genotypes, haplotype_likelihoods);

    CellPhylogeny two_group_phylogeny {CellPhylogeny::Group {0}};
    two_group_phylogeny.add_descendant(CellPhylogeny::Group {1}, 0);
    model::SingleCellPriorModel two_group_prior_model {std::move(two_group_phylogeny), *genotype_prior_model, mutation_model, cell_prior_params};
    model::SingleCellModel two_group_model {samples_, std::move(two_group_prior_model), model_parameters, config};
    auto two_group_inferences = two_group_model.evaluate(genotypes, haplotype_likelihoods);

    log(single_group_inferences, samples_, genotypes, debug_log_);
    log(two_group_inferences, samples_, genotypes, debug_log_);

    std::vector<model::SingleCellModel::Inferences> inferences {std::move(single_group_inferences), std::move(two_group_inferences)};
    return std::make_unique<Latents>(*this, haplotypes, std::move(genotypes), std::move(inferences));
}

boost::optional<double>
CellCaller::calculate_model_posterior(const std::vector<Haplotype>& haplotypes,
                                      const HaplotypeLikelihoodArray& haplotype_likelihoods,
                                      const Caller::Latents& latents) const
{
    return calculate_model_posterior(haplotypes, haplotype_likelihoods, dynamic_cast<const Latents&>(latents));
}

boost::optional<double>
CellCaller::calculate_model_posterior(const std::vector<Haplotype>& haplotypes,
                                      const HaplotypeLikelihoodArray& haplotype_likelihoods,
                                      const Latents& latents) const
{
    return boost::none;
}

std::vector<std::unique_ptr<octopus::VariantCall>>
CellCaller::call_variants(const std::vector<Variant>& candidates, const Caller::Latents& latents) const
{
    return call_variants(candidates, dynamic_cast<const Latents&>(latents));
}

namespace {

using GenotypeProbabilityMap = ProbabilityMatrix<Genotype<Haplotype>>::InnerMap;
using PopulationGenotypeProbabilityMap = ProbabilityMatrix<Genotype<Haplotype>>;

using VariantReference = std::reference_wrapper<const Variant>;
using VariantPosteriorVector = std::vector<std::pair<VariantReference, std::vector<Phred<double>>>>;

struct VariantCall : Mappable<VariantCall>
{
    VariantCall() = delete;
    VariantCall(const std::pair<VariantReference, std::vector<Phred<double>>>& p)
    : variant {p.first}
    , posteriors {p.second}
    {}
    VariantCall(const Variant& variant, std::vector<Phred<double>> posterior)
    : variant {variant}
    , posteriors {posterior}
    {}

    const GenomicRegion& mapped_region() const noexcept
    {
        return octopus::mapped_region(variant.get());
    }

    VariantReference variant;
    std::vector<Phred<double>> posteriors;
};

using VariantCalls = std::vector<VariantCall>;

struct GenotypeCall
{
    Genotype<Allele> genotype;
    Phred<double> posterior;
};

using GenotypeCalls = std::vector<std::vector<GenotypeCall>>;

// allele posterior calculations

using AlleleBools           = std::deque<bool>; // using std::deque because std::vector<bool> is evil
using GenotypePropertyBools = std::vector<AlleleBools>;

auto marginalise(const GenotypeProbabilityMap& genotype_posteriors,
                 const AlleleBools& contained_alleles)
{
    auto p = std::inner_product(std::cbegin(genotype_posteriors), std::cend(genotype_posteriors),
                                std::cbegin(contained_alleles), 0.0, std::plus<> {},
                                [] (const auto& p, const bool is_contained) {
                                    return is_contained ? 0.0 : p.second;
                                });
    return probability_to_phred(p);
}

auto compute_sample_allele_posteriors(const GenotypeProbabilityMap& genotype_posteriors,
                                      const GenotypePropertyBools& contained_alleles)
{
    std::vector<Phred<double>> result {};
    result.reserve(contained_alleles.size());
    for (const auto& allele : contained_alleles) {
        result.emplace_back(marginalise(genotype_posteriors, allele));
    }
    return result;
}

auto get_contained_alleles(const PopulationGenotypeProbabilityMap& genotype_posteriors,
                           const std::vector<Allele>& alleles)
{
    const auto num_genotypes = genotype_posteriors.size2();
    GenotypePropertyBools result {};
    if (num_genotypes == 0 || genotype_posteriors.empty1() || alleles.empty()) {
        return result;
    }
    result.reserve(alleles.size());
    const auto& test_sample   = genotype_posteriors.begin()->first;
    const auto genotype_begin = genotype_posteriors.begin(test_sample);
    const auto genotype_end   = genotype_posteriors.end(test_sample);
    for (const auto& allele : alleles) {
        result.emplace_back(num_genotypes);
        std::transform(genotype_begin, genotype_end, std::begin(result.back()),
                       [&] (const auto& p) { return contains(p.first, allele); });
    }
    return result;
}

using AllelePosteriorMatrix = std::vector<std::vector<Phred<double>>>;

auto compute_posteriors(const std::vector<SampleName>& samples,
                        const std::vector<Allele>& alleles,
                        const PopulationGenotypeProbabilityMap& genotype_posteriors)
{
    const auto contained_alleles = get_contained_alleles(genotype_posteriors, alleles);
    AllelePosteriorMatrix result {};
    result.reserve(genotype_posteriors.size1());
    for (const auto& sample : samples) {
        result.emplace_back(compute_sample_allele_posteriors(genotype_posteriors[sample], contained_alleles));
    }
    return result;
}

auto extract_ref_alleles(const std::vector<Variant>& variants)
{
    std::vector<Allele> result {};
    result.reserve(variants.size());
    std::transform(std::cbegin(variants), std::cend(variants), std::back_inserter(result),
                   [] (const auto& variant) { return variant.ref_allele(); });
    return result;
}

auto extract_alt_alleles(const std::vector<Variant>& variants)
{
    std::vector<Allele> result {};
    result.reserve(variants.size());
    std::transform(std::cbegin(variants), std::cend(variants), std::back_inserter(result),
                   [] (const auto& variant) { return variant.alt_allele(); });
    return result;
}

auto compute_posteriors(const std::vector<SampleName>& samples,
                        const std::vector<Variant>& variants,
                        const PopulationGenotypeProbabilityMap& genotype_posteriors)
{
    const auto allele_posteriors = compute_posteriors(samples, extract_alt_alleles(variants), genotype_posteriors);
    VariantPosteriorVector result {};
    result.reserve(variants.size());
    for (std::size_t i {0}; i < variants.size(); ++i) {
        std::vector<Phred<double>> sample_posteriors(samples.size());
        std::transform(std::cbegin(allele_posteriors), std::cend(allele_posteriors), std::begin(sample_posteriors),
                       [i] (const auto& ps) { return ps[i]; });
        result.emplace_back(variants[i], std::move(sample_posteriors));
    }
    return result;
}

// haplotype genotype calling

auto call_genotype(const PopulationGenotypeProbabilityMap::InnerMap& genotype_posteriors)
{
    return std::max_element(std::cbegin(genotype_posteriors), std::cend(genotype_posteriors),
                            [] (const auto& lhs, const auto& rhs) { return lhs.second < rhs.second; })->first;
}

auto call_genotypes(const std::vector<SampleName>& samples, const PopulationGenotypeProbabilityMap& genotype_posteriors)
{
    std::vector<Genotype<Haplotype>> result {};
    result.reserve(samples.size());
    for (const auto& sample : samples) {
        result.push_back(call_genotype(genotype_posteriors[sample]));
    }
    return result;
}

// variant calling

bool has_above(const std::vector<Phred<double>>& posteriors, const Phred<double> min_posterior)
{
    return std::any_of(std::cbegin(posteriors), std::cend(posteriors), [=] (auto p) { return p >= min_posterior; });
}

bool contains_alt(const Genotype<Haplotype>& genotype_call, const VariantReference& candidate)
{
    return includes(genotype_call, candidate.get().alt_allele());
}

bool contains_alt(const std::vector<Genotype<Haplotype>>& genotype_calls, const VariantReference& candidate)
{
    return std::any_of(std::cbegin(genotype_calls), std::cend(genotype_calls),
                       [&] (const auto& genotype) { return contains_alt(genotype, candidate); });
}

VariantCalls call_candidates(const VariantPosteriorVector& candidate_posteriors,
                             const std::vector<Genotype<Haplotype>>& genotype_calls,
                             const Phred<double> min_posterior)
{
    VariantCalls result {};
    result.reserve(candidate_posteriors.size());
    std::copy_if(std::cbegin(candidate_posteriors), std::cend(candidate_posteriors),
                 std::back_inserter(result),
                 [&genotype_calls, min_posterior] (const auto& p) {
                     return has_above(p.second, min_posterior) && contains_alt(genotype_calls, p.first);
                 });
    return result;
}

// allele genotype calling

auto marginalise(const Genotype<Allele>& genotype, const GenotypeProbabilityMap& genotype_posteriors)
{
    auto p = std::accumulate(std::cbegin(genotype_posteriors), std::cend(genotype_posteriors), 0.0,
                             [&genotype] (const double curr, const auto& p) {
                                 return curr + (contains(p.first, genotype) ? 0.0 : p.second);
                             });
    return probability_to_phred(p);
}

auto call_genotypes(const std::vector<SampleName>& samples,
                    const std::vector<Genotype<Haplotype>>& genotype_calls,
                    const PopulationGenotypeProbabilityMap& genotype_posteriors,
                    const std::vector<GenomicRegion>& variant_regions)
{
    GenotypeCalls result {};
    result.reserve(variant_regions.size());
    for (const auto& region : variant_regions) {
        std::vector<GenotypeCall> region_calls {};
        region_calls.reserve(samples.size());
        for (std::size_t s {0}; s < samples.size(); ++s) {
            auto genotype_chunk = copy<Allele>(genotype_calls[s], region);
            const auto posterior = marginalise(genotype_chunk, genotype_posteriors[samples[s]]);
            region_calls.push_back({std::move(genotype_chunk), posterior});
        }
        result.push_back(std::move(region_calls));
    }
    return result;
}

// output

octopus::VariantCall::GenotypeCall convert(GenotypeCall&& call)
{
    return octopus::VariantCall::GenotypeCall {std::move(call.genotype), call.posterior};
}

std::unique_ptr<octopus::VariantCall>
transform_call(const std::vector<SampleName>& samples,
               VariantCall&& variant_call,
               std::vector<GenotypeCall>&& sample_genotype_calls)
{
    std::vector<std::pair<SampleName, Call::GenotypeCall>> tmp {};
    tmp.reserve(samples.size());
    std::transform(std::cbegin(samples), std::cend(samples),
                   std::make_move_iterator(std::begin(sample_genotype_calls)),
                   std::back_inserter(tmp),
                   [] (const auto& sample, auto&& genotype) {
                       return std::make_pair(sample, convert(std::move(genotype)));
                   });
    auto quality = *std::max_element(std::cbegin(variant_call.posteriors), std::cend(variant_call.posteriors));
    return std::make_unique<CellVariantCall>(variant_call.variant.get(), std::move(tmp), quality);
}

auto transform_calls(const std::vector<SampleName>& samples,
                     VariantCalls&& variant_calls,
                     GenotypeCalls&& genotype_calls)
{
    std::vector<std::unique_ptr<octopus::VariantCall>> result {};
    result.reserve(variant_calls.size());
    std::transform(std::make_move_iterator(std::begin(variant_calls)), std::make_move_iterator(std::end(variant_calls)),
                   std::make_move_iterator(std::begin(genotype_calls)), std::back_inserter(result),
                   [&samples] (auto&& variant_call, auto&& genotype_call) {
                       return transform_call(samples, std::move(variant_call), std::move(genotype_call));
                   });
    return result;
}

} // namespace

std::vector<std::unique_ptr<octopus::VariantCall>>
CellCaller::call_variants(const std::vector<Variant>& candidates, const Latents& latents) const
{
    const auto& genotype_posteriors = *(latents.genotype_posteriors());
    const auto sample_candidate_posteriors = compute_posteriors(samples_, candidates, genotype_posteriors);
    const auto genotype_calls = call_genotypes(samples_, genotype_posteriors);
    auto variant_calls = call_candidates(sample_candidate_posteriors, genotype_calls, parameters_.min_variant_posterior);
    const auto called_regions = extract_regions(variant_calls);
    auto allele_genotype_calls = call_genotypes(samples_, genotype_calls, genotype_posteriors, called_regions);
    return transform_calls(samples_, std::move(variant_calls), std::move(allele_genotype_calls));
}

std::vector<std::unique_ptr<ReferenceCall>>
CellCaller::call_reference(const std::vector<Allele>& alleles, const Caller::Latents& latents, const ReadPileupMap& pileup) const
{
    return call_reference(alleles, dynamic_cast<const Latents&>(latents), pileup);
}

std::vector<std::unique_ptr<ReferenceCall>>
CellCaller::call_reference(const std::vector<Allele>& alleles, const Latents& latents, const ReadPileupMap& pileup) const
{
    return {};
}

std::unique_ptr<GenotypePriorModel> CellCaller::make_prior_model(const std::vector<Haplotype>& haplotypes) const
{
    if (parameters_.prior_model_params) {
        return std::make_unique<CoalescentGenotypePriorModel>(CoalescentModel {
        Haplotype {mapped_region(haplotypes.front()), reference_},
        *parameters_.prior_model_params, haplotypes.size(), CoalescentModel::CachingStrategy::address
        });
    } else {
        return std::make_unique<UniformGenotypePriorModel>();
    }
}

} // namespace octopus


// Author(s): Wieger Wesselink
// Copyright: see the accompanying file COPYING or copy at
// https://svn.win.tue.nl/trac/MCRL2/browser/trunk/COPYING
//
// Distributed under the Boost Software License, Version 1.0.
// (See accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//
/// \file process_test.cpp
/// \brief Add your file description here.

#include "mcrl2/process/find.h"
#include "mcrl2/process/parse.h"
#include "mcrl2/process/process_specification.h"
#include <boost/test/minimal.hpp>
#include <iostream>
#include <set>
#include <string>

using namespace mcrl2;
using namespace mcrl2::process;

const std::string SPEC1 =
  "act a;                  \n"
  "proc X = a;             \n"
  "init X;                 \n"
  ;

const std::string SPEC2 =
  "act a;                  \n"
  "proc X(i: Nat) = a.X(i);\n"
  "init X(2);              \n"
  ;

const std::string ABS_SPEC_LINEARIZED =
  "sort D = struct d1 | d2;                                                                                \n"
  "     Error = struct e;                                                                                \n"
  "                                                                                \n"
  "act  r1,s4: D;                                                                                \n"
  "     s2,r2,c2,s3,r3,c3: D # Bool;                                                                                \n"
  "     s3,r3,c3: Error;                                                                                \n"
  "     s5,r5,c5,s6,r6,c6: Bool;                                                                                \n"
  "     s6,r6,c6: Error;                                                                                \n"
  "     i;                                                                                \n"
  "                                                                                \n"
  "proc P(s31_S: Pos, d_S: D, b_S: Bool, s32_K: Pos, d_K: D, b_K: Bool, s33_L: Pos, b_L: Bool, s34_R: Pos, d_R: D, b_R: Bool) = \n"
  "       sum e1_S: Bool.                                                                                \n"
  "         ((s31_S == 3 && s33_L == 3) && if(e1_S, !b_S, b_S) == b_L) ->                                                       \n"
  "         c6(if(e1_S, !b_S, b_S)) .                                                                                \n"
  "         P(if(e1_S, 2, 1), if(e1_S, d_S, d2), if(e1_S, b_S, !b_S), s32_K, d_K, b_K, 1, false, s34_R, d_R, b_R)               \n"
  "     + (s31_S == 3 && s33_L == 4) ->                                                                                \n"
  "         c6(e) .                                                                                \n"
  "         P(2, d_S, b_S, s32_K, d_K, b_K, 1, false, s34_R, d_R, b_R)                                                          \n"
  "     + (s31_S == 2 && s32_K == 1) ->                                                                                \n"
  "         c2(d_S, b_S) .                                                                                \n"
  "         P(3, d_S, b_S, 2, d_S, b_S, s33_L, b_L, s34_R, d_R, b_R)                                                            \n"
  "     + sum e2_K: Bool.                                                                                \n"
  "         (s32_K == 2) ->                                                                                \n"
  "         i .                                                                                \n"
  "         P(s31_S, d_S, b_S, if(e2_K, 4, 3), if(e2_K, d2, d_K), if(e2_K, false, b_K), s33_L, b_L, s34_R, d_R, b_R)            \n"
  "     + sum e4_R: Bool.                                                                                \n"
  "         (s33_L == 1 && if(e4_R, s34_R == 4, s34_R == 3)) ->                                                                 \n"
  "         c5(if(e4_R, !b_R, b_R)) .                                                                                \n"
  "         P(s31_S, d_S, b_S, s32_K, d_K, b_K, 2, if(e4_R, !b_R, b_R), 1, d2, if(e4_R, b_R, !b_R))                             \n"
  "     + (s34_R == 2) ->                                                                                \n"
  "         s4(d_R) .                                                                                \n"
  "         P(s31_S, d_S, b_S, s32_K, d_K, b_K, s33_L, b_L, 3, d2, b_R)                                                         \n"
  "     + sum e3_L: Bool.                                                                                \n"
  "         (s33_L == 2) ->                                                                                \n"
  "         i .                                                                                \n"
  "         P(s31_S, d_S, b_S, s32_K, d_K, b_K, if(e3_L, 4, 3), if(e3_L, false, b_L), s34_R, d_R, b_R)                          \n"
  "     + (s32_K == 4 && s34_R == 1) ->                                                                                \n"
  "         c3(e) .                                                                                \n"
  "         P(s31_S, d_S, b_S, 1, d2, false, s33_L, b_L, 4, d2, b_R)                                                            \n"
  "     + sum e5_R: Bool.                                                                                \n"
  "         ((s32_K == 3 && s34_R == 1) && if(e5_R, b_R, !b_R) == b_K) ->                                                       \n"
  "         c3(d_K, if(e5_R, b_R, !b_R)) .                                                                                \n"
  "         P(s31_S, d_S, b_S, 1, d2, false, s33_L, b_L, if(e5_R, 2, 4), if(e5_R, d_K, d2), b_R)                                \n"
  "     + sum d3_S: D.                                                                                \n"
  "         (s31_S == 1) ->                                                                                \n"
  "         r1(d3_S) .                                                                                \n"
  "         P(2, d3_S, b_S, s32_K, d_K, b_K, s33_L, b_L, s34_R, d_R, b_R)                                                       \n"
  "     + true ->                                                                                \n"
  "         delta;                                                                                \n"
  "                                                                                \n"
  "init P(1, d2, true, 1, d2, false, 1, false, 1, d2, true);                                                                    \n"
  ;

// CASE?? specifications were borrowed from sumelm_test.

std::string CASE1 =
  "sort S = struct s1 | s2;\n"
  "map f : S -> Bool;\n"
  "act a : S # Bool;\n"
  "proc P = sum c : S, b : Bool . (b == f(c) && c == s2) -> a(c, b) . P;\n"
  "init P;\n"
  ;

std::string CASE2 =
  "act a,b;\n"
  "proc P(s3_P: Pos) = sum y_P: Int. (s3_P == 1) -> a . P(2)\n"
  "                  + (s3_P == 2) -> b . P(1);\n"
  "init P(1);\n"
  ;

std::string CASE3 =
  "act a;\n"
  "proc P = sum y:Int . (4 == y) -> a . P;\n"
  "init P;\n"
  ;

std::string CASE4 =
  "act a;\n"
  "proc P = sum y:Int . (y == 4) -> a . P;\n"
  "init P;\n"
  ;

std::string CASE5 =
  "act a,b:Int;\n"
  "proc P = sum y:Int . (y == 4) -> a(y)@y . b(y*2)@(y+1) . P;\n"
  "init P;\n"
  ;

std::string CASE6 =
  "act a;\n"
  "proc P = sum y:Int . (y == y + 1) -> a . P;\n"
  "init P;\n"
  ;

std::string CASE7 =
  "sort D = struct d1 | d2 | d3;\n"
  "map g : D -> D;\n"
  "act a;\n"
  "proc P(c:D) = sum d:D . sum e:D . sum f:D . (d == e && e == g(e) && e == f) -> a . P(d);\n"
  "init P(d1);\n"
  ;

std::string CASE8 =
  "sort D = struct d1 | d2 | d3;\n"
  "act a;\n"
  "proc P(c:D) = sum d:D . sum e:D . sum f:D . (d == e && d == f) -> a . P(d);\n"
  "init P(d1);\n"
  ;

std::string CASE9 =
  "proc P = sum y:Bool . y -> delta;\n"
  "init P;\n"
  ;

std::string CASE10 =
  "act a:Nat;\n"
  "proc P(n0: Nat) = sum n: Nat. (n == n0 && n == 1) -> a(n0) . P(n);\n"
  "init P(0);\n"
  ;

void test_process(const std::string& text)
{
  process_specification spec = parse_process_specification(text);
  std::set<data::sort_expression> sorts;
  process::find_sort_expressions(spec, std::inserter(sorts, sorts.end()));
  std::cerr << "sorts: " << data::pp(data::sort_expression_list(sorts.begin(), sorts.end())) << std::endl;
}

int test_main(int argc, char* argv[])
{
  test_process(CASE1);
  test_process(CASE2);
  test_process(CASE3);
  test_process(CASE4);
  test_process(CASE5);
  test_process(CASE6);
  test_process(CASE7);
  test_process(CASE8);
  test_process(CASE9);
  test_process(CASE10);
  test_process(SPEC1);
  test_process(SPEC2);
  test_process(ABS_SPEC_LINEARIZED);

  return 0;
}


//
// Copyright (c) 2014 CNRS
// Authors: Steve Tonneau (steve.tonneau@laas.fr)
//
// This file is part of hpp-rbprm.
// hpp-rbprm is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-rbprm 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 Lesser Public License for more details.  You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core  If not, see
// <http://www.gnu.org/licenses/>.

#ifndef HPP_IK_DEFINITIONS_HH
#define HPP_IK_DEFINITIONS_HH

#include <hpp/ik/config.hh>

#include <hpp/pinocchio/frame.hh>
#include <hpp/pinocchio/device.hh>
#include <hpp/core/config-projector.hh>

#include <Eigen/Core>

namespace hpp {

namespace ik {

typedef Eigen::Matrix <double, Eigen::Dynamic, 1> vector_t;
typedef Eigen::Vector3d vector3_t;
typedef Eigen::Quaterniond quat_t;

enum HPP_IK_DLLAPI MaintainConstraintType
{
    MAINTAIN_3D = 0,
    MAINTAIN_6D
};

struct HPP_IK_DLLAPI IkHelper{

    IkHelper(hpp::pinocchio::DevicePtr_t device, const double tolerance = 1e-5)
    : device_(device)
    , proj_(core::ConfigProjector::create(device, "proj", tolerance, 40 ))
    {
        device_->controlComputation(pinocchio::Computation_t(device_->computationFlag()
                                        | pinocchio::JOINT_POSITION | pinocchio::JACOBIAN | pinocchio::COM));
    }

     IkHelper(){}
    ~IkHelper(){}

    hpp::pinocchio::DevicePtr_t device_;
    core::ConfigProjectorPtr_t proj_;
};

struct HPP_IK_DLLAPI FrameMarker{

    FrameMarker(pinocchio::Frame& frame):
        frame_(frame),
        offset_(vector3_t::Zero()),
        normal_(vector3_t::Zero())
    {}

    FrameMarker(pinocchio::Frame& frame, const vector3_t& offset, const vector3_t& normal):
        frame_(frame),
        offset_(offset),
        normal_(normal)
    {}

    ~FrameMarker(){}

    pinocchio::Frame frame_;
    vector3_t offset_;
    vector3_t normal_;
};

} // namespace hpp
} // namespace ik
#endif // HPP_IK_DEFINITIONS_HH


#include <iostream>
#include <thread>
#include <tuple>
#include <random>
#include <chrono>
#include <functional>

#include "Part.h"
#include "Angle.h"
#include "Quaternion.h"
#include "World.h"
#include "BoundingBox.h"
#include "Vector.h"
#include "WeightedVector.h"

#include <boost/range/adaptors.hpp>
#include <boost/range/algorithm.hpp>
#include <boost/range/numeric.hpp>
#include <boost/range/algorithm_ext/erase.hpp>

template <class Eng>
Connection *selectRandomConnection(Part& part, Eng &engine)
{
	if (part.connections.reachable() == 0)
		return nullptr;

	std::uniform_int_distribution<ptrdiff_t> dist(0, part.connections.totalWeight() - 1);

	return &part.connections.getWeighted(dist(engine));
}

void movePart(Part& part, Connection* newc, const Connection* prevc)
{
	Quaternion targetQuat(prevc->angles());

	targetQuat = targetQuat * Quaternion{ 0, 0, 1, 0 };

	Angle newAngle = newc->angles();

	auto rotmat = newAngle.calcRotation(targetQuat);

	part.rotate(rotmat);
	Vector mov = Vector::diff(newc->origin(), prevc->origin());
	part.move(mov);

	auto iter = boost::find_if(part.connections, [newc](auto &con) {return &con == newc; });

	part.connections.setWeight(iter, 0);
}

void scaleToFit(Part& scaleable, Connection* scalec, const Connection* firstc, const Connection* secondc)
{
	double dist = (firstc->originKV() - secondc->originKV()).length();

	scaleable.scaleTo(dist);

	movePart(scaleable, scalec, firstc);
}

int main(int argc, char* argv[])
{
	//Part p(R"(f:\test\collisions\collision 1.vmf)");

	//std::vector<Solid> lhs, rhs;

	//const auto &worldspawn = *boost::find_if(p.entities, &Entity::entworldcmp);

	//for (auto &solid : worldspawn.solids)
	//{
	//	auto &side = solid.sides[0];

	//	if (side["material"] == "BRICK/BRICKFLOOR001A")
	//		lhs.push_back(solid);
	//	else if (side["material"] == "BRICK/BRICKWALL001A")
	//		rhs.push_back(solid);
	//}

	//std::cout << lhs.size() << ", " << rhs.size() << "\n";

	//for (auto &lhsSolid : lhs)
	//{
	//	for (auto &rhsSolid : rhs)
	//	{
	//		if (Solid::testCollision(lhsSolid, rhsSolid))
	//		{
	//			std::cout << "collision\n";
	//		}
	//	}
	//}

	//std::cin.get();

	//return 0;

	World randomWorld;

	randomWorld.addPart(Part(R"(f:\test\rndmap\room5.vmf)"));
	randomWorld.addPart(Part(R"(f:\test\testmap.vmf)"));
	randomWorld.addPart(Part(R"(f:\test\rndmap\room6.vmf)"));
	randomWorld.addPart(Part(R"(f:\test\rndmap\room1.vmf)"));
	randomWorld.addPart(Part(R"(f:\test\rndmap\room2.vmf)"));
	randomWorld.addPart(Part(R"(f:\test\rndmap\roomT.vmf)"));
	randomWorld.addPart(Part(R"(f:\test\rndmap\roomX.vmf)"));

	randomWorld.buildInitial(30);

	randomWorld.serialize().toFile(R"(f:\test\randomworld.vmf)");

	std::cin.get();

	return 0;
}