#include <k52/optimization/bounded_nelder_mead.h>

#ifdef BUILD_WITH_MPI

#include <boost/mpi.hpp>
#include <k52/parallel/mpi/constants.h>

#endif

#include <cmath>
#include <stdexcept>
#include <algorithm>
#include <fstream>
#include <iostream>

#include  <k52/common/constants.h>

using ::std::vector;
using ::k52::common::Constants;

namespace k52
{
namespace optimization
{

BoundedNelderMead::BoundedNelderMead(double l, double precision, double lower_bound, double upper_bound)
{
    lower_bound_ = lower_bound;
    upper_bound_ = upper_bound;
    precision_ = precision;
    l_ = l;
}

BoundedNelderMead* BoundedNelderMead::Clone() const
{
    return new BoundedNelderMead(l_, precision_, lower_bound_, upper_bound_);
}

std::string BoundedNelderMead::get_name() const
{
    return "Bounded Nelder Mead";
}

double BoundedNelderMead::get_lower_bound() const
{
    return lower_bound_;
}

double BoundedNelderMead::get_upper_bound() const
{
    return upper_bound_;
}

#ifdef BUILD_WITH_MPI
void BoundedNelderMead::Send(boost::mpi::communicator* communicator, int target) const
{
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, l_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, lower_bound_);
    communicator->send(target, k52::parallel::mpi::constants::kCommonTag, upper_bound_);
}

void BoundedNelderMead::Receive(boost::mpi::communicator* communicator, int source)
{
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, l_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, precision_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, lower_bound_);
    communicator->recv(source, k52::parallel::mpi::constants::kCommonTag, upper_bound_);
}
#endif

std::vector<double> BoundedNelderMead::FindOptimalParameters(const std::vector<double>& initial_parameters)
{
    //Iteration index
    size_t r = 0;

    //Build simplex based on initial_parameters
    vector< vector<double> > polygon = GetRegularSimplex(initial_parameters, l_);

    //count values
    vector<double> function_values = CountObjectiveFunctionValues(polygon);

    do
    {
        OutputPolygon(polygon);
        r++;

        size_t first_max_index = 0, second_max_index = 0, min_index = 0;
        //determine maximums and minimum
        GetIndexes(function_values, &first_max_index, &second_max_index, &min_index);

        double highest_value = function_values[first_max_index];
        double second_highest_value = function_values[second_max_index];
        double lowest_value = function_values[min_index];

        //determine center of mass
        vector<double> center_of_mass = GetCenterOfMass(polygon, first_max_index);

        //Reflect max point - we seek for minimum
        vector<double> reflected_point = Reflexion(center_of_mass, polygon[first_max_index]);
        CorrectByProjectingToBounds(&reflected_point);
        double reflected_point_value = CountSingleObjectiveFunctionValue(reflected_point);

        if (reflected_point_value < lowest_value)
        {
            vector<double> expanded_point = Expansion(center_of_mass, reflected_point);
            CorrectByProjectingToBounds(&expanded_point);
            double expanded_point_value = CountSingleObjectiveFunctionValue(expanded_point);

            if (expanded_point_value < reflected_point_value)
            {
                //Replace max with expanded
                polygon[first_max_index] = expanded_point;
                function_values[first_max_index] = expanded_point_value;
                continue;
            }
            else
            {
                //Replace max with reflected
                polygon[first_max_index] = reflected_point;
                function_values[first_max_index] = reflected_point_value;
                continue;
            }
        }
        else
        {
            if (reflected_point_value <= second_highest_value)
            {
                //Replace max with reflected
                polygon[first_max_index] = reflected_point;
                function_values[first_max_index] = reflected_point_value;
                continue;
            }
            else
            {
                if (reflected_point_value < highest_value)
                {
                    //Replace max with reflected
                    polygon[first_max_index] = reflected_point;
                    function_values[first_max_index] = reflected_point_value;
                    highest_value = reflected_point_value;
                }

                vector<double> contraction_point = Contraction(center_of_mass, polygon[first_max_index]);
                double contraction_point_value = CountSingleObjectiveFunctionValue(contraction_point);

                if (contraction_point_value > highest_value)
                {
                    Reduction(&polygon, min_index);
                    continue;
                }
                else
                {
                    //Replace max with contracted
                    polygon[first_max_index] = contraction_point;
                    function_values[first_max_index] = contraction_point_value;
                    continue;
                }
            }
        }
    } while (GetTerminationCriteria(r, function_values, polygon));

    size_t best_index = std::distance(function_values.begin(), std::max_element(function_values.begin(), function_values.end()));
    return polygon[best_index];
}

void BoundedNelderMead::CorrectByProjectingToBounds(vector<double>* point)
{
    for(size_t i = 0; i < point->size(); i++)
    {
        if( (*point)[i] > upper_bound_)
        {
            (*point)[i] = upper_bound_;
        }
        else if( (*point)[i] < lower_bound_)
        {
            (*point)[i] = lower_bound_;
        }
    }
}

vector<double> BoundedNelderMead::CountObjectiveFunctionValues(
    const vector< vector<double> >& parameters_values)
{
    size_t N = parameters_values.size();
    vector<double> counted_values(N);

    for(size_t i = 0; i < N; i++)
    {
        //As Nelder-Mead tends to minimize function we need to reverse it to find max
        counted_values[i] = CountObjectiveFunctionValueToMinimize(parameters_values[i]);
    }

    return counted_values;
}

double BoundedNelderMead::CountSingleObjectiveFunctionValue(
    const vector<double>& parameters)
{
    vector< vector<double> > cover(1);
    cover[0] = parameters;
    return CountObjectiveFunctionValues(cover)[0];
}

bool BoundedNelderMead::GetTerminationCriteria(
        size_t itreation_index,
        const std::vector<double>& function_values,
        const std::vector< std::vector<double> >& polygon)
{
    const size_t iterations_per_dimension = 1000;
    if (itreation_index > iterations_per_dimension * polygon[0].size())
    {
        std::cout << "WARNING: Exiting BDNM by iterations"
                " criteria!" << std::endl;
        return false;
    }

    if (WasSamePolygonBefore(polygon))
    {
        std::cout << "WARNING: Exiting BDNM because of a polygon loop,"
                " you migth have a special function behaviour." << std::endl;
        return false;
    }

    return CountDifferance(function_values) > precision_;
}

bool BoundedNelderMead::WasSamePolygonBefore(const std::vector< std::vector<double> >& polygon)
{
    bool was_same_before = false;

    for (std::list< std::vector< std::vector<double> > >::const_iterator it = previous_polygons_.begin();
         it != previous_polygons_.end();
         ++it)
    {
        if (CountPolygonDifferance(polygon, *it) < Constants::Eps)
        {
            was_same_before = true;
            break;
        }
    }

    if (previous_polygons_.size() >= max_previous_polygons_count)
    {
        previous_polygons_.pop_front();
    }
    previous_polygons_.push_back(polygon);

    return was_same_before;
}

void BoundedNelderMead::GetIndexes(const vector<double>& values, size_t* first_max_index, size_t* secound_max_index, size_t* min_index)
{
    if(values.size() < 2)
    {
        throw std::invalid_argument("values must have at least 2 elements");
    }

    if( values[0] > values[1] )
    {
        *first_max_index = 0;
        *secound_max_index = 1;
        *min_index = 1;
    }
    else
    {
        *first_max_index = 1;
        *secound_max_index = 0;
        *min_index = 0;
    }

    for (size_t i = 2; i<values.size(); i++)
    {
        if (values[i] > values[*first_max_index])
        {
            *secound_max_index = *first_max_index;
            *first_max_index = i;
        }
        else if (values[i]  > values[*secound_max_index])
        {
            *secound_max_index = i;
        }

        if( values[i] < values[*min_index])
        {
            *min_index = i;
        }
    }
}

vector< vector<double> > BoundedNelderMead::GetRegularSimplex(const vector<double>& base_point, double l)
{
    //Size of task
    size_t n = base_point.size();

    //For simplex points count
    double square_root_from_2 = std::sqrt(2.);
    double r1 = l * ( std::sqrt((double)(n+1)) + n - 1 ) / ( n * square_root_from_2 );
    double r2 = l * ( std::sqrt((double)(n+1)) - 1 ) / ( n * square_root_from_2 );

    vector< vector<double> > regular_simplex( n + 1 );
    regular_simplex[0] = base_point;

    for(size_t i = 1; i < n + 1; i++)
    {
        regular_simplex[i] = vector<double>(n);

        for(size_t j = 0; j < n; j++)
        {
            if( j == i - 1 )
            {
                regular_simplex[i][j] = base_point[j] + r1;
            }
            else
            {
                regular_simplex[i][j] = base_point[j] + r2;
            }
        }
    }
    return regular_simplex;
}

vector<double> BoundedNelderMead::Reflexion(const vector<double>& center_of_mass, const vector<double>& target_point)
{
    double reflection_coefficient = 1;
    vector<double> new_point (target_point.size());

    //Reflect
    for(size_t i = 0; i < target_point.size(); i++)
    {
        new_point[i] = center_of_mass[i] + reflection_coefficient * (center_of_mass[i] - target_point[i]);
    }

    return new_point;
}

vector<double> BoundedNelderMead::Expansion(const vector<double>& center_of_mass, const vector<double>& target_point)
{
    double expansion_coefficient = 2;
    vector<double> new_point (target_point.size());

    //Expand
    for(size_t i = 0; i < target_point.size(); i++)
    {
        new_point[i] = center_of_mass[i] + expansion_coefficient * (target_point[i] - center_of_mass[i]);
    }

    return new_point;
}

vector<double> BoundedNelderMead::Contraction(const vector<double>& center_of_mass, const vector<double>& target_point)
{
    double contraction_coefficient = 0.5;
    vector<double> new_point (target_point.size());

    //Contract
    for(size_t i = 0; i < target_point.size(); i++)
    {
        new_point[i] = center_of_mass[i] + contraction_coefficient * (target_point[i] - center_of_mass[i]);
    }

    return new_point;
}

void BoundedNelderMead::Reduction(vector< vector<double> >* polygon, size_t point_index)
{
    if(point_index >= polygon->size())
    {
        throw std::invalid_argument("Incorrect point_index");
    }

    double reduction_coefficient = 0.5;
    size_t n = (*polygon)[0].size();

    for(size_t i = 0; i < n +1; i++)
    {
        if( i != point_index )
        {
            for(size_t j = 0; j < n; j++)
            {
                (*polygon)[i][j] = (*polygon)[point_index][j] + reduction_coefficient *
                        ((*polygon)[i][j]  - (*polygon)[point_index][j]);
            }
        }
    }
}

double BoundedNelderMead::CountDifferance(const vector<double>& values)
{
    double summ = 0;
    for(size_t i = 0; i < values.size(); i++)
    {
        summ += values[i];
    }
    double averadge = summ / values.size();

    double square_summ = 0;
    for(size_t i = 0; i < values.size(); i++)
    {
        double diff = values[i] - averadge;
        square_summ += diff*diff;
    }
    return sqrt( square_summ / values.size());
}

double BoundedNelderMead::CountPolygonDifferance(
        const std::vector< std::vector<double> >& polygon,
        const std::vector< std::vector<double> >& previous_polygon)
{
    double summ = 0;
    for(size_t i = 0; i < polygon.size(); i++)
    {
        for(size_t j = 0; j < polygon[i].size(); j++)
        {
            summ += std::abs(polygon[i][j] - previous_polygon[i][j]);
        }
    }
    return summ;
}

vector<double> BoundedNelderMead::GetCenterOfMass(const vector< vector<double> >& polygon, size_t point_index)
{
    if(point_index>= polygon.size())
    {
        throw std::invalid_argument("Incorrect point_index");
    }

    size_t n = polygon[0].size();
    vector<double> center_of_mass(n);

    //TODO implement with valarray
    for(size_t i = 0; i < n; i++)
    {
        center_of_mass[i] = 0;

        for(size_t j = 0; j < n+1; j++)
        {
            if( j != point_index )
            {
                center_of_mass[i] += polygon[j][i];
            }
        }

        center_of_mass[i] /= n;
    }

    return center_of_mass;
}

void BoundedNelderMead::OutputPolygon(const vector< vector<double> >& polygon)
{
    std::ofstream polygon_output("polygon.plot", std::ofstream::app);
    polygon_output.precision(16);
    polygon_output<<std::endl;

    for(size_t i = 0; i < polygon.size(); i++)
    {
        for(size_t j = 0; j < polygon[i].size(); j++)
        {
            polygon_output << polygon[i][j] << " ";
        }
        polygon_output<<std::endl;
    }
}

}/* namespace optimization */
}/* namespace k52 */


// 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.

#ifndef variational_bayes_mixture_model_hpp
#define variational_bayes_mixture_model_hpp

#include <array>
#include <vector>
#include <algorithm>
#include <numeric>
#include <iterator>
#include <cstddef>
#include <utility>
#include <cassert>
#include <limits>

#include <boost/optional.hpp>
#include <boost/math/special_functions/digamma.hpp>

#include "utils/maths.hpp"
#include "utils/memory_footprint.hpp"
#include "core/models/haplotype_likelihood_array.hpp"

/**
 *
 * This file contains an implementation of the Variational Bayes mixture model
 * used by some genotype models. The notation follows the documentation.
 *
 */

namespace octopus { namespace model {

// Types needed for Variational Bayes model

struct VariationalBayesParameters
{
    double epsilon = 0.05;
    unsigned max_iterations = 1000;
    double save_memory = false;
};

using ProbabilityVector    = std::vector<double>;
using LogProbabilityVector = std::vector<double>;

template <std::size_t K>
using VBAlpha = std::array<double, K>;
template <std::size_t K>
using VBAlphaVector = std::vector<VBAlpha<K>>;

class VBReadLikelihoodArray
{
public:
    using BaseType = HaplotypeLikelihoodArray::LikelihoodVector;

    VBReadLikelihoodArray() = default;

    explicit VBReadLikelihoodArray(const BaseType&);

    VBReadLikelihoodArray(const VBReadLikelihoodArray&)            = default;
    VBReadLikelihoodArray& operator=(const VBReadLikelihoodArray&) = default;
    VBReadLikelihoodArray(VBReadLikelihoodArray&&)                 = default;
    VBReadLikelihoodArray& operator=(VBReadLikelihoodArray&&)      = default;

    ~VBReadLikelihoodArray() = default;

    void operator=(const BaseType&);
    void operator=(std::reference_wrapper<const BaseType>);
    std::size_t size() const noexcept;
    BaseType::const_iterator begin() const noexcept;
    BaseType::const_iterator end() const noexcept;
    double operator[](const std::size_t n) const noexcept;

private:
    const BaseType* likelihoods;
};

template <std::size_t K>
using VBGenotype = std::array<VBReadLikelihoodArray, K>; // One element per haplotype in genotype (i.e. K)
template <std::size_t K>
using VBGenotypeVector = std::vector<VBGenotype<K>>; // Per element per genotype
template <std::size_t K>
using VBReadLikelihoodMatrix = std::vector<VBGenotypeVector<K>>; // One element per sample

using VBTau = std::vector<double>; // One element per read
template <std::size_t K>
using VBResponsabilityVector = std::array<VBTau, K>; // One element per haplotype in genotype (i.e. K)
template <std::size_t K>
using VBResponsabilityMatrix = std::vector<VBResponsabilityVector<K>>; // One element per sample

template <std::size_t K>
struct VBLatents
{
    ProbabilityVector genotype_posteriors;
    LogProbabilityVector genotype_log_posteriors;
    VBAlphaVector<K> alphas;
    VBResponsabilityMatrix<K> responsabilities;
};

// Main VB method

namespace detail {

using VBExpandedLikelihood = std::vector<double>; // One element per genotype
using VBExpandedGenotype = std::vector<VBExpandedLikelihood>; // One element per read
template <std::size_t K>
using VBExpandedGenotypeVector = std::array<VBExpandedGenotype, K>; // One element per haplotype in genotype
template <std::size_t K>
using VBExpandedLikelihoodMatrix = std::vector<VBExpandedGenotypeVector<K>>; // One element per sample

template <std::size_t K>
auto invert(const VBGenotypeVector<K>& likelihoods)
{
    static_assert(K > 0, "K == 0");
    const auto num_genotypes = likelihoods.size();
    assert(num_genotypes > 0);
    const auto num_reads = likelihoods.front().front().size();
    VBExpandedGenotypeVector<K> result {};
    for (std::size_t k {0}; k < K; ++k) {
        result[k] = VBExpandedGenotype(num_reads, VBExpandedLikelihood(num_genotypes));
        for (std::size_t n {0}; n < num_reads; ++n) {
            for (std::size_t g {0}; g < num_genotypes; ++g) {
                result[k][n][g] = likelihoods[g][k][n];
            }
        }
    }
    return result;
}

template <std::size_t K>
auto invert(const VBReadLikelihoodMatrix<K>& matrix)
{
    VBExpandedLikelihoodMatrix<K> result {};
    result.reserve(matrix.size());
    std::transform(std::cbegin(matrix), std::cend(matrix), std::back_inserter(result),
                   [] (const auto& v) { return invert(v); });
    return result;
}

inline ProbabilityVector& exp(const LogProbabilityVector& log_probabilities, ProbabilityVector& result) noexcept
{
    std::transform(std::cbegin(log_probabilities), std::cend(log_probabilities), std::begin(result),
                   [] (const auto lp) noexcept { return std::exp(lp); });
    return result;
}

inline ProbabilityVector exp(const LogProbabilityVector& log_probabilities)
{
    ProbabilityVector result(log_probabilities.size());
    return exp(log_probabilities, result);
}

inline auto sum(const VBAlpha<2>& alpha) noexcept
{
    return alpha[0] + alpha[1];
}

inline auto sum(const VBAlpha<3>& alpha) noexcept
{
    return alpha[0] + alpha[1] + alpha[2];
}

template <std::size_t K>
auto sum(const VBAlpha<K>& alpha) noexcept
{
    using T = typename VBAlpha<K>::value_type;
    return std::accumulate(std::cbegin(alpha), std::cend(alpha), T {0});
}

template <typename T>
inline auto digamma_diff(const T a, const T b)
{
    using boost::math::digamma;
    return digamma(a) - digamma(b);
}

template <typename T>
inline T log_sum_exp(const std::array<T, 1>& logs)
{
    return logs[0];
}

template <typename T>
inline T log_sum_exp(const std::array<T, 2>& logs)
{
    return maths::log_sum_exp(logs[0], logs[1]);
}

template <typename T>
inline T log_sum_exp(const std::array<T, 3>& logs)
{
    return maths::log_sum_exp(logs[0], logs[1], logs[2]);
}

template <typename T, std::size_t K>
T log_sum_exp(const std::array<T, K>& logs)
{
    return maths::log_sum_exp(logs);
}

template <std::size_t K>
auto count_reads(const VBGenotypeVector<K>& likelihoods) noexcept
{
    return likelihoods[0][0].size();
}

template <std::size_t K>
auto count_reads(const VBExpandedGenotypeVector<K>& likelihoods) noexcept
{
    return likelihoods[0].size();
}

template <std::size_t K>
auto marginalise(const ProbabilityVector& distribution, const VBGenotypeVector<K>& likelihoods,
                 const unsigned k, const std::size_t n) noexcept
{
    using T = ProbabilityVector::value_type;
    return std::inner_product(std::cbegin(distribution), std::cend(distribution),
                              std::cbegin(likelihoods), T {0}, std::plus<> {},
                              [k, n] (const auto p, const auto& haplotype_likelihoods) noexcept {
                                  return p * haplotype_likelihoods[k][n];
                              });
}

template <typename T1, typename T2>
auto inner_product(const T1& lhs, const T2& rhs) noexcept
{
    assert(std::distance(std::cbegin(lhs), std::cend(lhs)) == std::distance(std::cbegin(rhs), std::cend(rhs)));
    using T = typename T1::value_type;
    return std::inner_product(std::cbegin(lhs), std::cend(lhs), std::cbegin(rhs), T {0});
}

template <std::size_t K>
auto marginalise(const ProbabilityVector& distribution, const VBExpandedGenotypeVector<K>& likelihoods,
                 const unsigned k, const std::size_t n) noexcept
{
    return inner_product(distribution, likelihoods[k][n]);
}

template <std::size_t K, typename VBLikelihoodVector_>
VBResponsabilityVector<K>
init_responsabilities(const VBAlpha<K>& prior_alphas,
                      const ProbabilityVector& genotype_probabilities,
                      const VBLikelihoodVector_& read_likelihoods)
{
    using T = typename VBAlpha<K>::value_type;
    std::array<T, K> al; // no need to keep recomputing this
    const auto a0 = sum(prior_alphas);
    for (unsigned k {0}; k < K; ++k) {
        al[k] = digamma_diff(prior_alphas[k], a0);
    }
    const auto N = count_reads(read_likelihoods);
    VBResponsabilityVector<K> result {};
    for (auto& tau : result) tau.resize(N);
    std::array<T, K> ln_rho;
    for (std::size_t n {0}; n < N; ++n) {
        for (unsigned k {0}; k < K; ++k) {
            ln_rho[k] = al[k] + marginalise(genotype_probabilities, read_likelihoods, k, n);
        }
        const auto ln_rho_norm = log_sum_exp(ln_rho);
        for (unsigned k {0}; k < K; ++k) {
            result[k][n] = std::exp(ln_rho[k] - ln_rho_norm);
        }
    }
    return result;
}

template <std::size_t K, typename VBLikelihoodMatrix>
VBResponsabilityMatrix<K>
init_responsabilities(const VBAlphaVector<K>& prior_alphas,
                      const ProbabilityVector& genotype_probabilities,
                      const VBLikelihoodMatrix& read_likelihoods)
{
    const auto S = read_likelihoods.size(); // num samples
    VBResponsabilityMatrix<K> result {};
    result.reserve(S);
    for (std::size_t s {0}; s < S; ++s) {
        result.push_back(init_responsabilities(prior_alphas[s], genotype_probabilities, read_likelihoods[s]));
    }
    return result;
}

template <std::size_t K, typename VBLikelihoodVector_>
void update_responsabilities(VBResponsabilityVector<K>& result,
                             const VBAlpha<K>& posterior_alphas,
                             const ProbabilityVector& genotype_probabilities,
                             const VBLikelihoodVector_& read_likelihoods)
{
    using T = typename VBAlpha<K>::value_type;
    std::array<T, K> al;
    const auto a0 = sum(posterior_alphas);
    for (unsigned k {0}; k < K; ++k) {
        al[k] = digamma_diff(posterior_alphas[k], a0);
    }
    const auto N = count_reads(read_likelihoods);
    std::array<T, K> ln_rho;
    for (std::size_t n {0}; n < N; ++n) {
        for (unsigned k {0}; k < K; ++k) {
            ln_rho[k] = al[k] + marginalise(genotype_probabilities, read_likelihoods, k, n);
        }
        const auto ln_rho_norm = log_sum_exp(ln_rho);
        for (unsigned k {0}; k < K; ++k) {
            result[k][n] = std::exp(ln_rho[k] - ln_rho_norm);
        }
    }
}

// same as init_responsabilities but in-place
template <std::size_t K, typename VBLikelihoodMatrix>
void update_responsabilities(VBResponsabilityMatrix<K>& result,
                             const VBAlphaVector<K>& posterior_alphas,
                             const ProbabilityVector& genotype_probabilities,
                             const VBLikelihoodMatrix& read_likelihoods)
{
    const auto S = read_likelihoods.size();
    for (std::size_t s {0}; s < S; ++s) {
        update_responsabilities(result[s], posterior_alphas[s], genotype_probabilities, read_likelihoods[s]);
    }
}

template <typename T>
inline auto sum(const std::vector<T>& values) noexcept
{
    return std::accumulate(std::cbegin(values), std::cend(values), T {});
}

template <std::size_t K>
void update_alpha(VBAlpha<K>& alpha, const VBAlpha<K>& prior_alpha,
                  const VBResponsabilityVector<K>& taus) noexcept
{
    for (unsigned k {0}; k < K; ++k) {
        alpha[k] = prior_alpha[k] + sum(taus[k]);
    }
}

template <std::size_t K>
void update_alphas(VBAlphaVector<K>& alphas, const VBAlphaVector<K>& prior_alphas,
                   const VBResponsabilityMatrix<K>& responsabilities) noexcept
{
    const auto S = alphas.size();
    assert(S == prior_alphas.size() && S == responsabilities.size());
    for (std::size_t s {0}; s < S; ++s) {
        update_alpha(alphas[s], prior_alphas[s], responsabilities[s]);
    }
}

inline auto marginalise(const VBTau& responsabilities, const VBReadLikelihoodArray& likelihoods) noexcept
{
    assert(responsabilities.size() == likelihoods.size()); // num reads
    return inner_product(responsabilities, likelihoods);
}

template <std::size_t K>
auto marginalise(const VBResponsabilityVector<K>& responsabilities,
                 const VBGenotype<K>& read_likelihoods) noexcept
{
    double result {0};
    for (unsigned k {0}; k < K; ++k) {
        result += marginalise(responsabilities[k], read_likelihoods[k]);
    }
    return result;
}

template <std::size_t K>
auto marginalise(const VBResponsabilityMatrix<K>& responsabilities,
                 const VBReadLikelihoodMatrix<K>& read_likelihoods,
                 const std::size_t g) noexcept
{
    double result {0};
    const auto S = read_likelihoods.size(); // num samples
    assert(S == responsabilities.size());
    for (std::size_t s {0}; s < S; ++s) {
        result += marginalise(responsabilities[s], read_likelihoods[s][g]);
    }
    return result;
}

template <std::size_t K>
void update_genotype_log_posteriors(LogProbabilityVector& result,
                                    const LogProbabilityVector& genotype_log_priors,
                                    const VBResponsabilityMatrix<K>& responsabilities,
                                    const VBReadLikelihoodMatrix<K>& read_likelihoods)
{
    const auto G = result.size();
    for (std::size_t g {0}; g < G; ++g) {
        result[g] = genotype_log_priors[g] + marginalise(responsabilities, read_likelihoods, g);
    }
    maths::normalise_logs(result);
}

inline auto entropy(const VBTau& tau) noexcept
{
    using T = VBTau::value_type;
    return -std::accumulate(std::cbegin(tau), std::cend(tau), T {0},
                            [] (const auto curr, const auto t) noexcept { return curr + (t * std::log(t)); });
}

// E [ln q(Z_s)]
template <std::size_t K>
auto sum_entropies(const VBResponsabilityVector<K>& taus) noexcept
{
    using T = VBTau::value_type;
    return std::accumulate(std::cbegin(taus), std::cend(taus), T {0},
                           [] (const auto curr, const auto& tau) noexcept { return curr + entropy(tau); });
}

template <std::size_t K>
auto calculate_evidence_lower_bound(const VBAlphaVector<K>& prior_alphas,
                                    const VBAlphaVector<K>& posterior_alphas,
                                    const LogProbabilityVector& genotype_log_priors,
                                    const ProbabilityVector& genotype_posteriors,
                                    const LogProbabilityVector& genotype_log_posteriors,
                                    const VBResponsabilityMatrix<K>& taus,
                                    const VBReadLikelihoodMatrix<K>& log_likelihoods,
                                    const boost::optional<double> max_posterior_skip = boost::none)
{
    const auto G = genotype_log_priors.size();
    const auto S = log_likelihoods.size();
    double result {0};
    for (std::size_t g {0}; g < G; ++g) {
        if (!max_posterior_skip || genotype_posteriors[g] >= *max_posterior_skip) {
            auto w = genotype_log_priors[g] - genotype_log_posteriors[g];
            for (std::size_t s {0}; s < S; ++s) {
                w += marginalise(taus[s], log_likelihoods[s][g]);
            }
            result += genotype_posteriors[g] * w;
        }
    }
    for (std::size_t s {0}; s < S; ++s) {
        result += (maths::log_beta(posterior_alphas[s]) - maths::log_beta(prior_alphas[s]));
        result += sum_entropies(taus[s]);
    }
    return result;
}

// Main algorithm - single seed

// Starting iteration with given genotype_log_posteriors
template <std::size_t K, typename VBLikelihoodMatrix1, typename VBLikelihoodMatrix2>
VBLatents<K>
run_variational_bayes(const VBAlphaVector<K>& prior_alphas,
                      const LogProbabilityVector& genotype_log_priors,
                      const VBLikelihoodMatrix1& log_likelihoods1,
                      const VBLikelihoodMatrix2& log_likelihoods2,
                      LogProbabilityVector genotype_log_posteriors,
                      const VariationalBayesParameters& params)
{
    assert(!prior_alphas.empty());
    assert(!genotype_log_priors.empty());
    assert(!log_likelihoods1.empty());
    assert(log_likelihoods1.size() == log_likelihoods2.size());
    assert(prior_alphas.size() == log_likelihoods1.size()); // num samples
    assert(log_likelihoods1.front().size() == genotype_log_priors.size()); // num genotypes
    assert(params.max_iterations > 0);
    auto genotype_posteriors = exp(genotype_log_posteriors);
    auto posterior_alphas = prior_alphas;
    auto responsabilities = init_responsabilities<K>(posterior_alphas, genotype_posteriors, log_likelihoods2);
    assert(responsabilities.size() == log_likelihoods1.size()); // num samples
    auto prev_evidence = std::numeric_limits<double>::lowest();
    for (unsigned i {0}; i < params.max_iterations; ++i) {
        update_genotype_log_posteriors(genotype_log_posteriors, genotype_log_priors, responsabilities, log_likelihoods1);
        exp(genotype_log_posteriors, genotype_posteriors);
        update_alphas(posterior_alphas, prior_alphas, responsabilities);
        auto curr_evidence = calculate_evidence_lower_bound(prior_alphas, posterior_alphas, genotype_log_priors,
                                                            genotype_posteriors, genotype_log_posteriors, responsabilities,
                                                            log_likelihoods1, 1e-10);
        if (curr_evidence <= prev_evidence || (curr_evidence - prev_evidence) < params.epsilon) break;
        prev_evidence = curr_evidence;
        update_responsabilities(responsabilities, posterior_alphas, genotype_posteriors, log_likelihoods2);
    }
    return VBLatents<K> {
        std::move(genotype_posteriors), std::move(genotype_log_posteriors),
        std::move(posterior_alphas), std::move(responsabilities)
    };
}

// Not using inverted log likelihoods
template <std::size_t K>
VBLatents<K>
run_variational_bayes(const VBAlphaVector<K>& prior_alphas,
                      const LogProbabilityVector& genotype_log_priors,
                      const VBReadLikelihoodMatrix<K>& log_likelihoods,
                      LogProbabilityVector genotype_log_posteriors,
                      const VariationalBayesParameters& params)
{
    return run_variational_bayes(prior_alphas, genotype_log_posteriors, log_likelihoods,
                                 log_likelihoods, genotype_log_posteriors, params);
}

// Main algorithm - multiple seed

template <std::size_t K>
bool run_vb_with_matrix_inversion(const VBReadLikelihoodMatrix<K>& log_likelihoods,
                                  const VariationalBayesParameters& params,
                                  const std::vector<LogProbabilityVector>& seeds) noexcept
{
    return !params.save_memory;
}

template <std::size_t K>
std::vector<VBLatents<K>>
run_variational_bayes(const VBAlphaVector<K>& prior_alphas,
                      const LogProbabilityVector& genotype_log_priors,
                      const VBReadLikelihoodMatrix<K>& log_likelihoods,
                      const VariationalBayesParameters& params,
                      std::vector<LogProbabilityVector>&& seeds)
{
    std::vector<VBLatents<K>> result {};
    result.reserve(seeds.size());
    if (run_vb_with_matrix_inversion(log_likelihoods, params, seeds)) {
        const auto inverted_log_likelihoods = invert(log_likelihoods);
        for (auto& seed : seeds) {
            result.push_back(detail::run_variational_bayes(prior_alphas, genotype_log_priors,
                                                           log_likelihoods, inverted_log_likelihoods,
                                                           std::move(seed), params));
        }
    } else {
        for (auto& seed : seeds) {
            result.push_back(detail::run_variational_bayes(prior_alphas, genotype_log_priors,
                                                           log_likelihoods,
                                                           std::move(seed), params));
        }
    }
    return result;
}

// lower-bound calculation

template <std::size_t K>
auto calculate_evidence_lower_bound(const VBAlphaVector<K>& prior_alphas,
                                    const LogProbabilityVector& genotype_log_priors,
                                    const VBReadLikelihoodMatrix<K>& log_likelihoods,
                                    const VBLatents<K>& latents)
{
    return calculate_evidence_lower_bound(prior_alphas, latents.alphas, genotype_log_priors,
                                          latents.genotype_posteriors, latents.genotype_log_posteriors,
                                          latents.responsabilities, log_likelihoods);

}

template <std::size_t K>
void check_normalisation(VBLatents<K>& latents)
{
    const auto total_posterior = std::accumulate(std::cbegin(latents.genotype_posteriors), std::cend(latents.genotype_posteriors), 0.0);
    if (total_posterior > 1.0) {
        for (auto& p : latents.genotype_posteriors) p /= total_posterior;
    }
}

template <std::size_t K>
std::pair<VBLatents<K>, double>
get_max_evidence_latents(const VBAlphaVector<K>& prior_alphas,
                         const LogProbabilityVector& genotype_log_priors,
                         const VBReadLikelihoodMatrix<K>& log_likelihoods,
                         std::vector<VBLatents<K>>&& latents)
{
    std::vector<double> seed_evidences(latents.size());
    std::transform(std::cbegin(latents), std::cend(latents), std::begin(seed_evidences),
                   [&] (const auto& seed_latents) {
                       return calculate_evidence_lower_bound(prior_alphas, genotype_log_priors, log_likelihoods, seed_latents);
                   });
    const auto max_itr = std::max_element(std::cbegin(seed_evidences), std::cend(seed_evidences));
    const auto max_idx = std::distance(std::cbegin(seed_evidences), max_itr);
    return std::make_pair(std::move(latents[max_idx]), *max_itr);
}

} // namespace detail

template <std::size_t K>
std::pair<VBLatents<K>, double>
run_variational_bayes(const VBAlphaVector<K>& prior_alphas,
                      const LogProbabilityVector& genotype_log_priors,
                      const VBReadLikelihoodMatrix<K>& log_likelihoods,
                      const VariationalBayesParameters& params,
                      std::vector<LogProbabilityVector> seeds)
{
    assert(!seeds.empty());
    auto latents = detail::run_variational_bayes(prior_alphas, genotype_log_priors, log_likelihoods, params, std::move(seeds));
    auto result = detail::get_max_evidence_latents(prior_alphas, genotype_log_priors, log_likelihoods, std::move(latents));
    detail::check_normalisation(result.first);
    return result;
}

inline VBReadLikelihoodArray::VBReadLikelihoodArray(const BaseType& underlying_likelihoods)
: likelihoods{std::addressof(underlying_likelihoods)} {}

inline void VBReadLikelihoodArray::operator=(const BaseType& other)
{
    likelihoods = std::addressof(other);
}

inline void VBReadLikelihoodArray::operator=(std::reference_wrapper<const BaseType> other)
{
    likelihoods = std::addressof(other.get());
}

inline std::size_t VBReadLikelihoodArray::size() const noexcept
{
    return likelihoods->size();
}

inline VBReadLikelihoodArray::BaseType::const_iterator VBReadLikelihoodArray::begin() const noexcept
{
    return likelihoods->begin();
}

inline VBReadLikelihoodArray::BaseType::const_iterator VBReadLikelihoodArray::end() const noexcept
{
    return likelihoods->end();
}

inline double VBReadLikelihoodArray::operator[](const std::size_t n) const noexcept
{
    return likelihoods->operator[](n);
}

template <std::size_t K>
std::pair<VBLatents<K>, double>
run_variational_bayes(const VBAlphaVector<K>& prior_alphas,
                      const LogProbabilityVector& genotype_log_priors,
                      const VBReadLikelihoodMatrix<K>& log_likelihoods,
                      const VariationalBayesParameters& params,
                      std::vector<LogProbabilityVector> seeds);

template <std::size_t K>
MemoryFootprint
estimate_memory_requirement(const std::vector<SampleName>& samples,
                            const HaplotypeLikelihoodArray& likelihoods,
                            const std::size_t num_genotypes,
                            VariationalBayesParameters params)
{
    std::size_t bytes {};
    for (const auto& sample : samples) {
        bytes += sizeof(VBReadLikelihoodMatrix<K>);
        bytes += sizeof(VBGenotypeVector<K>) * num_genotypes;
        bytes += sizeof(VBResponsabilityMatrix<K>);
        const auto num_likelihoods = likelihoods.num_likelihoods(sample);
        const auto tau_bytes = num_likelihoods * sizeof(VBTau::value_type);
        bytes += tau_bytes * K + sizeof(VBResponsabilityVector<K>);
        if (!params.save_memory) {
            bytes += sizeof(detail::VBExpandedLikelihoodMatrix<K>);
            auto inverse_bytes = sizeof(detail::VBExpandedLikelihood::value_type) * num_genotypes + sizeof(detail::VBExpandedLikelihood);
            inverse_bytes *= num_likelihoods;
            inverse_bytes += sizeof(detail::VBExpandedGenotype);
            bytes += K * inverse_bytes + sizeof(detail::VBExpandedGenotypeVector<K>);
        }
    }
    return MemoryFootprint {bytes};
};

} // namespace model
} // namespace octopus

#endif


#include <data/Geometry.hpp>
#include <engine/Neighbours.hpp>
#include <engine/Vectormath.hpp>
#include <utility/Exception.hpp>

#include <Eigen/Core>
#include <Eigen/Geometry>

#include "Qhull.h"
#include "QhullFacetList.h"
#include "QhullVertexSet.h"

#include <fmt/ostream.h>

#include <random>
#include <array>

namespace Data
{
    Geometry::Geometry(std::vector<Vector3> bravais_vectors, intfield n_cells,
                        std::vector<Vector3> cell_atoms,  Basis_Cell_Composition cell_composition,
                        scalar lattice_constant, Pinning pinning, Defects defects) :
        bravais_vectors(bravais_vectors), n_cells(n_cells), n_cell_atoms(cell_atoms.size()),
        cell_atoms(cell_atoms), cell_composition(cell_composition), lattice_constant(lattice_constant),
        nos(cell_atoms.size() * n_cells[0] * n_cells[1] * n_cells[2]),
        nos_nonvacant(cell_atoms.size() * n_cells[0] * n_cells[1] * n_cells[2]),
        n_cells_total(n_cells[0] * n_cells[1] * n_cells[2]),
        pinning(pinning), defects(defects)
    {
        // Generate positions and atom types
        this->positions = vectorfield(this->nos);
        this->generatePositions();

        // Calculate some useful info
        this->calculateBounds();
        this->calculateUnitCellBounds();
        this->calculateDimensionality();

        // Calculate center of the System
        this->center = 0.5 * (this->bounds_min + this->bounds_max);

        // Generate default atom_types, mu_s and pinning masks
        this->atom_types        = intfield(this->nos, 0);
        this->mu_s              = scalarfield(this->nos, 1);
        this->mask_unpinned     = intfield(this->nos, 1);
        this->mask_pinned_cells = vectorfield(this->nos, { 0,0,0 });

        // Set atom types, mu_s
        this->applyCellComposition();

        // Apply additional pinned sites
        for( int isite=0; isite < pinning.sites.size(); ++isite )
        {
            auto& site = pinning.sites[isite];
            int ispin = site.i + Engine::Vectormath::idx_from_translations(
                        this->n_cells, this->n_cell_atoms,
                        {site.translations[0], site.translations[1], site.translations[2]} );
            this->mask_unpinned[ispin] = 0;
            this->mask_pinned_cells[ispin] = pinning.spins[isite];
        }

        // Apply additional defect sites
        for (int i = 0; i < defects.sites.size(); ++i)
        {
            auto& defect = defects.sites[i];
            int ispin = defects.sites[i].i + Engine::Vectormath::idx_from_translations(
                        this->n_cells, this->n_cell_atoms,
                        {defect.translations[0], defect.translations[1], defect.translations[2]} );
            this->atom_types[ispin] = defects.types[i];
            this->mu_s[ispin] = 0.0;
        }

        // Calculate the type of geometry
        this->calculateGeometryType();

        // For updates of triangulation and tetrahedra
        this->last_update_n_cell_step = -1;
        this->last_update_n_cells = intfield(3, -1);
    }

    void Geometry::generatePositions()
    {
        const scalar epsilon = 1e-6;

        // Check for erronous input placing two spins on the same location
        int max_a = std::min(10, n_cells[0]);
        int max_b = std::min(10, n_cells[1]);
        int max_c = std::min(10, n_cells[2]);
        Vector3 diff;
        for (int i = 0; i < n_cell_atoms; ++i)
        {
            for (int j = 0; j < n_cell_atoms; ++j)
            {
                for (int da = -max_a; da <= max_a; ++da)
                {
                    for (int db = -max_b; db <= max_b; ++db)
                    {
                        for (int dc = -max_c; dc <= max_c; ++dc)
                        {
                            // Norm is zero if translated basis atom is at position of another basis atom
                            diff = cell_atoms[i] - ( cell_atoms[j] + Vector3{scalar(da), scalar(db), scalar(dc)} );

                            if( (i != j || da != 0 || db != 0 || dc != 0) &&
                                std::abs(diff[0]) < epsilon &&
                                std::abs(diff[1]) < epsilon &&
                                std::abs(diff[2]) < epsilon )
                            {
                                Vector3 position = lattice_constant * (
                                      (da + cell_atoms[i][0]) * bravais_vectors[0]
                                    + (db + cell_atoms[i][1]) * bravais_vectors[1]
                                    + (dc + cell_atoms[i][2]) * bravais_vectors[2] );
                                std::string message = fmt::format(
                                    "Unable to initialize Spin-System, since 2 spins occupy the same space"
                                    "within a margin of {} at absolute position ({}).\n"
                                    "Index combination: i={} j={}, translations=({}, {}, {}).\n"
                                    "Please check the config file!", epsilon, position.transpose(), i, j, da, db, dc);
                                spirit_throw(Utility::Exception_Classifier::System_not_Initialized, Utility::Log_Level::Severe, message);
                            }
                        }
                    }
                }
            }
        }

        // Generate positions
        for (int dc = 0; dc < n_cells[2]; ++dc)
        {
            for (int db = 0; db < n_cells[1]; ++db)
            {
                for (int da = 0; da < n_cells[0]; ++da)
                {
                    for (int iatom = 0; iatom < n_cell_atoms; ++iatom)
                    {
                        int ispin = iatom
                            + dc * n_cell_atoms * n_cells[1] * n_cells[0]
                            + db * n_cell_atoms * n_cells[0]
                            + da * n_cell_atoms;

                        positions[ispin] = lattice_constant * (
                                  (da + cell_atoms[iatom][0]) * bravais_vectors[0]
                                + (db + cell_atoms[iatom][1]) * bravais_vectors[1]
                                + (dc + cell_atoms[iatom][2]) * bravais_vectors[2] );
                    }
                }
            }
        }
    }

    std::vector<tetrahedron_t> compute_delaunay_triangulation_3D(const std::vector<vector3_t> & points)
    try
    {
        const int ndim = 3;
        std::vector<tetrahedron_t> tetrahedra;
        tetrahedron_t tmp_tetrahedron;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        orgQhull::QhullFacetList facet_list = qhull.facetList();
        for(const auto & facet : facet_list)
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_tetrahedron[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                tetrahedra.push_back(tmp_tetrahedron);
            }
        }
        return tetrahedra;
    }
    catch( ... )
    {
        spirit_handle_exception_core( "Could not compute 3D Delaunay triangulation of the Geometry. Probably Qhull threw an exception." );
        return std::vector<tetrahedron_t>(0);
    }

    std::vector<triangle_t> compute_delaunay_triangulation_2D(const std::vector<vector2_t> & points)
    try
    {
        const int ndim = 2;
        std::vector<triangle_t> triangles;
        triangle_t tmp_triangle;
        int *current_index;

        orgQhull::Qhull qhull;
        qhull.runQhull("", ndim, points.size(), (coordT *) points.data(),  "d Qt Qbb Qz");
        for(const auto & facet : qhull.facetList())
        {
            if(!facet.isUpperDelaunay())
            {
                current_index = &tmp_triangle[0];
                for(const auto & vertex : facet.vertices())
                {
                    *current_index++ = vertex.point().id();
                }
                triangles.push_back(tmp_triangle);
            }
        }
        return triangles;
    }
    catch( ... )
    {
        spirit_handle_exception_core( "Could not compute 2D Delaunay triangulation of the Geometry. Probably Qhull threw an exception." );
        return std::vector<triangle_t>(0);
    }

    const std::vector<triangle_t>& Geometry::triangulation(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if ( (n_cells[0]/n_cell_step < 2 && n_cells[0] > 1) ||
             (n_cells[1]/n_cell_step < 2 && n_cells[1] > 1) ||
             (n_cells[2]/n_cell_step < 2 && n_cells[2] > 1) )
        {
            _triangulation.clear();
            return _triangulation;
        }

        // 2D: triangulation
        if (this->dimensionality == 2)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                _triangulation.clear();

                std::vector<vector2_t> points;
                points.resize(positions.size());

                int icell = 0, idx;
                for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                {
                    for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                    {
                        for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                        {
                            for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                            {
                                idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                points[icell].x = double(positions[idx][0]);
                                points[icell].y = double(positions[idx][1]);
                                ++icell;
                            }
                        }
                    }
                }
                _triangulation = compute_delaunay_triangulation_2D(points);
            }
        }// endif 2D
        // 0D, 1D and 3D give no triangulation
        else
        {
            _triangulation.clear();
        }
        return _triangulation;
    }

    const std::vector<tetrahedron_t>& Geometry::tetrahedra(int n_cell_step)
    {
        // Only every n_cell_step'th cell is used. So we check if there is still enough cells in all
        //      directions. Note: when visualising, 'n_cell_step' can be used to e.g. olny visualise
        //      every 2nd spin.
        if (n_cells[0]/n_cell_step < 2 || n_cells[1]/n_cell_step < 2 || n_cells[2]/n_cell_step < 2)
        {
            _tetrahedra.clear();
            return _tetrahedra;
        }

        // 3D: Tetrahedra
        if (this->dimensionality == 3)
        {
            // Check if the tetrahedra for this combination of n_cells and n_cell_step has already been calculated
            if (this->last_update_n_cell_step != n_cell_step ||
                this->last_update_n_cells[0]  != n_cells[0]  ||
                this->last_update_n_cells[1]  != n_cells[1]  ||
                this->last_update_n_cells[2]  != n_cells[2]  )
            {
                this->last_update_n_cell_step = n_cell_step;
                this->last_update_n_cells[0]  = n_cells[0];
                this->last_update_n_cells[1]  = n_cells[1];
                this->last_update_n_cells[2]  = n_cells[2];

                // If we have only one spin in the basis our lattice is a simple regular geometry
                bool is_simple_regular_geometry = n_cell_atoms == 1;

                // If we have a simple regular geometry everything can be calculated by hand
                if (is_simple_regular_geometry)
                {
                    _tetrahedra.clear();
                    int cell_indices[] = {
                        0, 1, 5, 3,
                        1, 3, 2, 5,
                        3, 2, 5, 6,
                        7, 6, 5, 3,
                        4, 7, 5, 3,
                        0, 4, 3, 5
                        };
                    int x_offset = 1;
                    int y_offset = n_cells[0]/n_cell_step;
                    int z_offset = n_cells[0]/n_cell_step*n_cells[1]/n_cell_step;
                    int offsets[] = {
                        0, x_offset, x_offset+y_offset, y_offset,
                        z_offset, x_offset+z_offset, x_offset+y_offset+z_offset, y_offset+z_offset
                        };

                    for (int ix = 0; ix < (n_cells[0]-1)/n_cell_step; ix++)
                    {
                        for (int iy = 0; iy < (n_cells[1]-1)/n_cell_step; iy++)
                        {
                            for (int iz = 0; iz < (n_cells[2]-1)/n_cell_step; iz++)
                            {
                                int base_index = ix*x_offset+iy*y_offset+iz*z_offset;
                                for (int j = 0; j < 6; j++)
                                {
                                    tetrahedron_t tetrahedron;
                                    for (int k = 0; k < 4; k++)
                                    {
                                        int index = base_index + offsets[cell_indices[j*4+k]];
                                        tetrahedron[k] = index;
                                    }
                                    _tetrahedra.push_back(tetrahedron);
                                }
                            }
                        }
                    }
                }
                // For general basis cells we calculate the Delaunay tetrahedra
                else
                {
                    std::vector<vector3_t> points;
                    points.resize(positions.size());

                    int icell = 0, idx;
                    for (int cell_c=0; cell_c<n_cells[2]; cell_c+=n_cell_step)
                    {
                        for (int cell_b=0; cell_b<n_cells[1]; cell_b+=n_cell_step)
                        {
                            for (int cell_a=0; cell_a<n_cells[0]; cell_a+=n_cell_step)
                            {
                                for (int ibasis=0; ibasis < n_cell_atoms; ++ibasis)
                                {
                                    idx = ibasis + n_cell_atoms*cell_a + n_cell_atoms*n_cells[0]*cell_b + n_cell_atoms*n_cells[0]*n_cells[1]*cell_c;
                                    points[icell].x = double(positions[idx][0]);
                                    points[icell].y = double(positions[idx][1]);
                                    points[icell].z = double(positions[idx][2]);
                                    ++icell;
                                }
                            }
                        }
                    }
                    _tetrahedra = compute_delaunay_triangulation_3D(points);
                }
            }
        } // endif 3D
        // 0-2 D gives no tetrahedra
        else
        {
            _tetrahedra.clear();
        }
        return _tetrahedra;
    }

    std::vector<Vector3> Geometry::BravaisVectorsSC()
    {
        return { { scalar(1), scalar(0), scalar(0) },
                 { scalar(0), scalar(1), scalar(0) },
                 { scalar(0), scalar(0), scalar(1) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsFCC()
    {
        return { { scalar(0.5), scalar(0.0), scalar(0.5) },
                 { scalar(0.5), scalar(0.5), scalar(0.0) },
                 { scalar(0.0), scalar(0.5), scalar(0.5) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsBCC()
    {
        return { { scalar( 0.5), scalar( 0.5), scalar(-0.5) },
                 { scalar(-0.5), scalar( 0.5), scalar(-0.5) },
                 { scalar( 0.5), scalar(-0.5), scalar(-0.5) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D60()
    {
        return { { scalar(0.5*std::sqrt(3)), scalar(-0.5), scalar(0) },
                 { scalar(0.5*std::sqrt(3)), scalar( 0.5), scalar(0) },
                 { scalar(0),                scalar( 0),   scalar(1) } };
    }

    std::vector<Vector3> Geometry::BravaisVectorsHex2D120()
    {
        return { { scalar(0.5), scalar(-0.5*std::sqrt(3)), scalar(0) },
                 { scalar(0.5), scalar( 0.5*std::sqrt(3)), scalar(0) },
                 { scalar(0),   scalar( 0),                scalar(1) } };
    }

    void Geometry::applyCellComposition()
    {
        int N  = this->n_cell_atoms;
        int Na = this->n_cells[0];
        int Nb = this->n_cells[1];
        int Nc = this->n_cells[2];
        int ispin, iatom, atom_type;
        scalar concentration, rvalue;
        std::vector<bool> visited(N);

        std::mt19937 prng;
        std::uniform_real_distribution<scalar> distribution;
        if( this->cell_composition.disordered )
        {
            // TODO: the seed should be a parameter and the instance a member of this class
            prng = std::mt19937(2006);
            distribution = std::uniform_real_distribution<scalar>(0, 1);
            // In the disordered case, unvisited atoms will be vacancies
            this->atom_types = intfield(nos, -1);
        }

        for (int na = 0; na < Na; ++na)
        {
            for (int nb = 0; nb < Nb; ++nb)
            {
                for (int nc = 0; nc < Nc; ++nc)
                {
                    std::fill(visited.begin(), visited.end(), false);

                    for (int icomposition = 0; icomposition < this->cell_composition.iatom.size(); ++icomposition)
                    {
                        iatom = this->cell_composition.iatom[icomposition];

                        if( !visited[iatom] )
                        {
                            ispin = N*na + N*Na*nb + N*Na*Nb*nc + iatom;

                            // In the disordered case, we only visit an atom if the dice will it
                            if( this->cell_composition.disordered )
                            {
                                concentration = this->cell_composition.concentration[icomposition];
                                rvalue = distribution(prng);
                                if( rvalue <= concentration )
                                {
                                    this->atom_types[ispin] = this->cell_composition.atom_type[icomposition];
                                    this->mu_s[ispin]       = this->cell_composition.mu_s[icomposition];
                                    visited[iatom] = true;
                                    if( this->atom_types[ispin] < 0 )
                                        --this->nos_nonvacant;
                                }
                            }
                            // In the ordered case, we visit every atom
                            else
                            {
                                this->atom_types[ispin] = this->cell_composition.atom_type[icomposition];
                                this->mu_s[ispin]       = this->cell_composition.mu_s[icomposition];
                                visited[iatom] = true;
                                if( this->atom_types[ispin] < 0 )
                                    --this->nos_nonvacant;
                            }

                            // Pinning of boundary layers
                            if( (na < pinning.na_left || na >= Na - pinning.na_right) ||
                                (nb < pinning.nb_left || nb >= Nb - pinning.nb_right) ||
                                (nc < pinning.nc_left || nc >= Nc - pinning.nc_right) )
                            {
                                // Pinned cells
                                this->mask_unpinned[ispin] = 0;
                                this->mask_pinned_cells[ispin] = pinning.pinned_cell[iatom];
                            }
                        }
                    }
                }
            }
        }
    }

    void Geometry::calculateDimensionality()
    {
        int dims_basis = 0, dims_translations = 0;
        Vector3 test_vec_basis, test_vec_translations;

        const scalar epsilon = 1e-6;

        // ----- Find dimensionality of the basis -----
        if     ( n_cell_atoms == 1 )
            dims_basis = 0;
        else if( n_cell_atoms == 2 )
        {
            dims_basis = 1;
            test_vec_basis = positions[0] - positions[1];
        }
        else
        {
            // Get basis atoms relative to the first atom
            Vector3 v0 = positions[0];
            std::vector<Vector3> b_vectors(n_cell_atoms-1);
            for( int i = 1; i < n_cell_atoms; ++i )
                b_vectors[i-1] = (positions[i] - v0).normalized();

            // Calculate basis dimensionality
            // test vec is along line
            test_vec_basis = b_vectors[0];
            //      is it 1D?
            int n_parallel = 0;
            for( unsigned int i = 1; i < b_vectors.size(); ++i )
            {
                if( std::abs(b_vectors[i].dot(test_vec_basis)) - 1 < epsilon )
                    ++n_parallel;
                // Else n_parallel will give us the last parallel vector
                // Also the if-statement for dims_basis=1 wont be met
                else
                    break;
            }
            if( n_parallel == b_vectors.size() - 1 )
            {
                dims_basis = 1;
            }
            else
            {
                // test vec is normal to plane
                test_vec_basis = b_vectors[0].cross(b_vectors[n_parallel+1]);
                //      is it 2D?
                int n_in_plane = 0;
                for( unsigned int i = 2; i < b_vectors.size(); ++i )
                {
                    if (std::abs(b_vectors[i].dot(test_vec_basis)) < epsilon)
                        ++n_in_plane;
                }
                if( n_in_plane == b_vectors.size() - 2 )
                    dims_basis = 2;
                else
                {
                    this->dimensionality = 3;
                    return;
                }
            }
        }

        // ----- Find dimensionality of the translations -----
        //      The following are zero if the corresponding pair is parallel or antiparallel
        double t01, t02, t12;
        t01 = std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[1].normalized())) - 1.0;
        t02 = std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[2].normalized())) - 1.0;
        t12 = std::abs(bravais_vectors[1].normalized().dot(bravais_vectors[2].normalized())) - 1.0;
        //      Check if pairs are linearly independent
        int n_independent_pairs = 0;
        if( t01 < epsilon && n_cells[0] > 1 && n_cells[1] > 1 ) ++n_independent_pairs;
        if( t02 < epsilon && n_cells[0] > 1 && n_cells[2] > 1 ) ++n_independent_pairs;
        if( t12 < epsilon && n_cells[1] > 1 && n_cells[2] > 1 ) ++n_independent_pairs;
        //      Calculate translations dimensionality
        if( n_cells[0] == 1 && n_cells[1] == 1 && n_cells[2] == 1 )
        {
            dims_translations = 0;
        }
        else if( n_independent_pairs == 0 )
        {
            dims_translations = 1;
            // Test if vec is along the line
            for (int i=0; i<3; ++i) if (n_cells[i] > 1) test_vec_translations = bravais_vectors[i];
        }
        else if( n_independent_pairs < 3 )
        {
            dims_translations = 2;
            // Test if vec is normal to plane
            int n = 0;
            std::vector<Vector3> plane(2);
            for( int i = 0; i < 3; ++i )
            {
                if( n_cells[i] > 1 )
                {
                    plane[n] = bravais_vectors[i];
                    ++n;
                }
            }
            test_vec_translations = plane[0].cross(plane[1]);
        }
        else
        {
            this->dimensionality = 3;
            return;
        }

        // ----- Calculate dimensionality of system -----
        test_vec_basis.normalize();
        test_vec_translations.normalize();
        //      If one dimensionality is zero, only the other counts
        if( dims_basis == 0 )
        {
            this->dimensionality = dims_translations;
            return;
        }
        else if( dims_translations == 0 )
        {
            this->dimensionality = dims_basis;
            return;
        }
        //      If both are linear or both are planar, the test vectors should be (anti)parallel if the geometry is 1D or 2D
        else if (dims_basis == dims_translations)
        {
            if( std::abs(test_vec_basis.dot(test_vec_translations)) - 1 < epsilon )
            {
                this->dimensionality = dims_basis;
                return;
            }
            else if( dims_basis == 1 )
            {
                this->dimensionality = 2;
                return;
            }
            else if( dims_basis == 2 )
            {
                this->dimensionality = 3;
                return;
            }
        }
        //      If one is linear (1D), and the other planar (2D) then the test vectors should be orthogonal if the geometry is 2D
        else if( (dims_basis == 1 && dims_translations == 2) || (dims_basis == 2 && dims_translations == 1) )
        {
            if( std::abs(test_vec_basis.dot(test_vec_translations)) < epsilon )
            {
                this->dimensionality = 2;
                return;
            }
            else
            {
                this->dimensionality = 3;
                return;
            }
        }
    }

    void Geometry::calculateBounds()
    {
        this->bounds_max.setZero();
        this->bounds_min.setZero();
        for (int iatom = 0; iatom < nos; ++iatom)
        {
            for (int dim = 0; dim < 3; ++dim)
            {
                if (this->positions[iatom][dim] < this->bounds_min[dim]) this->bounds_min[dim] = this->positions[iatom][dim];
                if (this->positions[iatom][dim] > this->bounds_max[dim]) this->bounds_max[dim] = this->positions[iatom][dim];
            }
        }
    }

    void Geometry::calculateUnitCellBounds()
    {
        this->cell_bounds_max.setZero();
        this->cell_bounds_min.setZero();
        for (unsigned int ivec = 0; ivec < this->bravais_vectors.size(); ++ivec)
        {
            for (int iatom = 0; iatom < this->n_cell_atoms; ++iatom)
            {
                auto neighbour1 = this->positions[iatom] + this->lattice_constant * this->bravais_vectors[ivec];
                auto neighbour2 = this->positions[iatom] - this->lattice_constant * this->bravais_vectors[ivec];
                for (int dim = 0; dim < 3; ++dim)
                {
                    if (neighbour1[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour1[dim];
                    if (neighbour1[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour1[dim];
                    if (neighbour2[dim] < this->cell_bounds_min[dim]) this->cell_bounds_min[dim] = neighbour2[dim];
                    if (neighbour2[dim] > this->cell_bounds_max[dim]) this->cell_bounds_max[dim] = neighbour2[dim];
                }
            }
        }
        this->cell_bounds_min *= 0.5;
        this->cell_bounds_max *= 0.5;
    }

    void Geometry::calculateGeometryType()
    {
        const scalar epsilon = 1e-6;

        // Automatically try to determine GeometryType
        // Single-atom unit cell
        if (cell_atoms.size() == 1)
        {
            // If the basis vectors are orthogonal, it is a rectilinear lattice
            if (std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[1].normalized())) < epsilon &&
                std::abs(bravais_vectors[0].normalized().dot(bravais_vectors[2].normalized())) < epsilon)
            {
                // If equidistant it is simple cubic
                if (bravais_vectors[0].norm() == bravais_vectors[1].norm() == bravais_vectors[2].norm())
                    this->classifier = BravaisLatticeType::SC;
                // Otherwise only rectilinear
                else
                    this->classifier = BravaisLatticeType::Rectilinear;
            }
        }
        // Regular unit cell with multiple atoms (e.g. bcc, fcc, hex)
        //else if (n_cell_atoms == 2)
        // Irregular unit cells arranged on a lattice (e.g. B20 or custom)
        /*else if (n_cells[0] > 1 || n_cells[1] > 1 || n_cells[2] > 1)
        {
            this->classifier = BravaisLatticeType::Lattice;
        }*/
        // A single irregular unit cell
        else
        {
            this->classifier = BravaisLatticeType::Irregular;
        }
    }

    void Geometry::Apply_Pinning(vectorfield & vf)
    {
        #if defined(SPIRIT_ENABLE_PINNING)
        int N  = this->n_cell_atoms;
        int Na = this->n_cells[0];
        int Nb = this->n_cells[1];
        int Nc = this->n_cells[2];
        int ispin;

        for (int iatom = 0; iatom < N; ++iatom)
        {
            for (int na = 0; na < Na; ++na)
            {
                for (int nb = 0; nb < Nb; ++nb)
                {
                    for (int nc = 0; nc < Nc; ++nc)
                    {
                        ispin = N*na + N*Na*nb + N*Na*Nb*nc + iatom;
                        if (!this->mask_unpinned[ispin])
                            vf[ispin] = this->mask_pinned_cells[ispin];
                    }
                }
            }
        }
        #endif
    }
}


/**
 * Copyright (c) 2017 Melown Technologies SE
 *
 * 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.
 */
#include <algorithm>

#include <boost/format.hpp>
#include <boost/filesystem.hpp>

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>

#include "math/math.hpp"
#include "math/geometry.hpp"
#include "math/transform.hpp"
#include "imgproc/scanconversion.hpp"
#include "imgproc/binterpolate.hpp"

#include "geometry/faceclip.hpp"
#include "geometry/pointindex.hpp"
#include "imgproc/uvpack.hpp"

#include "geometry/mesh.hpp"
#include "geometry/meshop.hpp"

#include "tileset.hpp"
#include "merge.hpp"
#include "io.hpp"

#include "geometry/binmesh.hpp"

namespace ublas = boost::numeric::ublas;
namespace va = vtslibs;
namespace fs = boost::filesystem;

#ifndef BUILDSYS_CUSTOMER_BUILD
//#define DEBUG 1 // save debug images
#endif

namespace cv {

// comparison operators on cv::Point2 and cv::Point3 for va::PointIndex
template<typename T>
inline bool operator< (const Point_<T> &lhs, const Point_<T> &rhs)
{
    if (lhs.x != rhs.x) return lhs.x < rhs.x;
    return lhs.y < rhs.y;
}

template<typename T>
inline bool operator< (const Point3_<T> &lhs, const Point3_<T> &rhs)
{
    if (lhs.x != rhs.x) return lhs.x < rhs.x;
    if (lhs.y != rhs.y) return lhs.y < rhs.y;
    return lhs.z < rhs.z;
}

} // namespace cv

namespace vtslibs { namespace vts0 {

namespace {

const auto QBufferCvType = CV_16S;
typedef signed short QBufferCType;

double triangleArea(const math::Point3 &a, const math::Point3 &b,
                    const math::Point3 &c)
{
    return norm_2(math::crossProduct(b - a, c - a)) * 0.5;
}

//! Returns a trasformation from tile local coordinates to qbuffer coordinates.
//!
math::Matrix4 tileToBuffer(const math::Size2f &tileSize, int bufferSize)
{
    math::Matrix4 trafo(ublas::identity_matrix<double>(4));
    trafo(0,0) = double(bufferSize) / tileSize.width;
    trafo(1,1) = double(bufferSize) / tileSize.height;
    trafo(0,3) = trafo(1,3) = double(bufferSize) / 2;
    return trafo;
}

//! "Draws" all faces of a tile into the qbuffer, i.e., stores the tile's index
//! everywhere its mesh lies.
//!
void rasterizeTile(const Tile &tile, const math::Matrix4 &trafo,
                   int index, cv::Mat &qbuffer)
{
    std::vector<imgproc::Scanline> scanlines;

    // draw all faces into the qbuffer
    for (const auto face : tile.mesh.facets)
    {
        cv::Point3f tri[3];
        for (int i = 0; i < 3; i++) {
            auto pt(transform(trafo, tile.mesh.vertices[face.v[i]]));
            tri[i] = {float(pt(0)), float(pt(1)), float(pt(2))};
        }

        scanlines.clear();
        imgproc::scanConvertTriangle(tri, 0, qbuffer.rows, scanlines);

        for (const auto& sl : scanlines) {
            imgproc::processScanline(sl, 0, qbuffer.cols,
                [&](int x, int y, float)
                    {
                        qbuffer.at<QBufferCType>(y, x) = index;
                    } );
        }
    }
}

//! Calculates tile Inverse Geometry Coarseness, defined as the ratio of mesh area and area
//! of the covered tile part
//!
double tileInvGeometryCoarseness(const Tile &tile
                                 , const math::Size2f &tileSize)
{
    // calculate the total area of the faces in both the XYZ and UV spaces
    double xyzArea(0);
    const Mesh &mesh(tile.mesh);

    //clip mesh to the tile extents
    geometry::opencv::ClipTriangle::list triangles;
    for(auto &face : mesh.facets){
        triangles.emplace_back( 0, 0,
                                mesh.vertices[face.v[0]],
                                mesh.vertices[face.v[1]],
                                mesh.vertices[face.v[2]],
                                math::Point2(0,0),
                                math::Point2(0,0),
                                math::Point2(0,0));
    }

    //cut small border of the tile to get rid of the skirts
    double eps = 0.0001;
    math::Size2f halfsize(0.5*tileSize.width - tileSize.width*eps
                          , 0.5*tileSize.height - tileSize.height*eps);

    geometry::opencv::ClipPlane planes[4];
    planes[0] = {+1.,  0., 0., halfsize.width};
    planes[1] = {-1.,  0., 0., halfsize.width};
    planes[2] = { 0., +1., 0., halfsize.height};
    planes[3] = { 0., -1., 0., halfsize.height};

    for (int i = 0; i < 4; i++) {
        triangles = geometry::opencv::clipTriangles(triangles, planes[i]);
    }

    //compute the mesh area from clipped triangles
    for (const auto &face : triangles)
    {
        xyzArea += triangleArea(
            math::Point3(face.pos[0].x, face.pos[0].y, face.pos[0].z),
            math::Point3(face.pos[1].x, face.pos[1].y, face.pos[1].z),
            math::Point3(face.pos[2].x, face.pos[2].y, face.pos[2].z));
    }

    const int cBSize(512);
    cv::Mat cbuffer(cBSize, cBSize, CV_32S, cv::Scalar(0));

    math::Matrix4 trafo(tileToBuffer(tileSize, cBSize));
    rasterizeTile(tile, trafo,1, cbuffer);
    double coveredArea = static_cast<double>(cv::countNonZero(cbuffer))
                            /(cBSize*cBSize);

    return coveredArea ? xyzArea/coveredArea : 0;
}

//! Returns true if x and y are valid column and row indices, respectively.
//!
bool validMatPos(int x, int y, const cv::Mat &mat)
{
    return x >= 0 && x < mat.cols && y >= 0 && y < mat.rows;
}

//! Adjusts x and y so that they are valid column and row indices, respectively.
//!
void clampMatPos(int &x, int &y, const cv::Mat &mat)
{
    if (x < 0) x = 0;
    else if (x >= mat.cols) x = mat.cols-1;

    if (y < 0) y = 0;
    else if (y >= mat.rows) y = mat.rows-1;
}

template <int Steps, int Radius>
void close(cv::Mat &mat)
{
    auto element
        (cv::getStructuringElement
         (cv::MORPH_RECT, { 1 + 2 * Radius, 1 + 2 * Radius }));
    cv::Mat tmp(mat.size(), mat.type());
    cv::morphologyEx(mat, tmp, cv::MORPH_CLOSE, element
                     , { -1, -1 }, Steps, cv::BORDER_REPLICATE);
    swap(mat, tmp);
}

//! Returns true if at least one element in the area of the qbuffer
//! corresponding to the given face belongs to tile number 'index'.
//!
bool faceCovered(const Mesh &mesh, const Mesh::Facet &face,
                 const math::Matrix4 &trafo, int index,
                 const cv::Mat/*<QBufferCType>*/ &qbuffer)
{
    cv::Point3f tri[3];
    for (int i = 0; i < 3; i++) {
        auto pt(transform(trafo, mesh.vertices[face.v[i]]));
        tri[i] = {float(pt(0)), float(pt(1)), float(pt(2))};
    }

    std::vector<imgproc::Scanline> scanlines;
    imgproc::scanConvertTriangle(tri, 0, qbuffer.rows, scanlines);

    for (const auto& sl : scanlines) {
        bool covered(false);
        imgproc::processScanline(sl, 0, qbuffer.cols, [&](int x, int y, float)
        {
            if (qbuffer.at<QBufferCType>(y, x) == index) {
                covered = true;
            }
        });
        if (covered) { return true; }
    }

    // do one more check in case the triangle is thinner than one pixel
    for (int i = 0; i < 3; i++) {
        int x(round(tri[i].x)), y(round(tri[i].y));
        clampMatPos(x, y, qbuffer);
        if (qbuffer.at<QBufferCType>(y, x) == index) { return true; }
    }

    return false;
}

//! Draws a face into a matrix. Used to mark useful areas of an atlas.
//!
void markFace(const geometry::opencv::ClipTriangle &face
              , cv::Mat/*<char>*/ &mat)
{
    cv::Point3f tri[3];
    for (int i = 0; i < 3; i++) {
        tri[i] = {face.uv[i].x, face.uv[i].y, 0};

        // make sure the vertices are always marked
        int x(round(tri[i].x)), y(round(tri[i].y));
        if (validMatPos(x, y, mat)) {
            mat.at<char>(y, x) = 1;
        }
    }

    std::vector<imgproc::Scanline> scanlines;
    imgproc::scanConvertTriangle(tri, 0, mat.rows, scanlines);

    for (const auto& sl : scanlines) {
        imgproc::processScanline(sl, 0, mat.cols,
            [&](int x, int y, float){ mat.at<char>(y, x) = 1; } );
    }
}

//! A wrapper for cv::findContours to solve for the 1-pixel border that the cv
//! function ignores.
//!
void findContours(const cv::Mat &mat,
                  std::vector<std::vector<cv::Point> > &contours)
{
    cv::Mat tmp(mat.rows + 2, mat.cols + 2, mat.type(), cv::Scalar(0));
    cv::Rect rect(1, 1, mat.cols, mat.rows);
    mat.copyTo(cv::Mat(tmp, rect));

    cv::findContours(tmp, contours, cv::RETR_EXTERNAL
                     , cv::CHAIN_APPROX_NONE
                     , cv::Point(-1, -1));
}

//! Clips faces to tile boundaries.
//!
void clipFaces(geometry::opencv::ClipTriangle::list &faces
               , const math::Size2f &tileSize)
{
    math::Size2f ts2(tileSize.width / 2.0, tileSize.height / 2.0);

    geometry::opencv::ClipPlane planes[4] = {
        { 1.,  0., 0., ts2.width},
        {-1.,  0., 0., ts2.width},
        { 0.,  1., 0., ts2.height},
        { 0., -1., 0., ts2.height}
    };

    for (int i = 0; i < 4; i++) {
        faces = clipTriangles(faces, planes[i]);
    }
}

//! Convert from normalized to pixel-based texture coordinates.
//!
imgproc::UVCoord denormalizeUV(const math::Point3d &uv, cv::Size texSize)
{
    return {float(uv(0) * texSize.width),
            //float(texSize.height-1 - uv(1)*texSize.height)
            float((1.0 - uv(1)) * texSize.height)};
}

//! Convert from pixel-based to normalized texture coordinates.
//!
math::Point3d normalizeUV(const imgproc::UVCoord &uv, cv::Size texSize)
{
    return {double(uv.x) / texSize.width,
            //(texSize.height-1 - double(uv.y)) / texSize.height,
            1.0 - double(uv.y) / texSize.height,
            0.0};
}

// helpers to convert between math points and cv points
cv::Point3d vertex(const math::Point3d &v)
    { return {v(0), v(1), v(2)}; }

math::Point3d vertex(const cv::Point3d &v)
    { return {v.x, v.y, v.z}; }

cv::Point2d tCoord(const math::Point2 &t)
    { return { t(0), t(1)}; }

math::Point2 tCoord(const cv::Point2d &t)
    { return {t.x, t.y}; }

//! Calculate a bounding rectangle of a list of points in the UV space.
//!
imgproc::UVRect makeRect(const std::vector<cv::Point> &points)
{
    imgproc::UVRect rect;
    for (const auto &pt : points) {
        rect.update(imgproc::UVCoord(pt.x, pt.y));
    }
    return rect;
}

//! Finds a UV rectangle containing the given face.
//!
unsigned findRect(const geometry::opencv::ClipTriangle &face
                  , const std::vector<imgproc::UVRect> &rects)
{
    const auto &pt(face.uv[0]);
    for (unsigned i = 0; i < rects.size(); i++)
    {
        const imgproc::UVRect &r(rects[i]);
        const float safety(0.51);

        if (pt.x > (r.min.x - safety) && pt.x < (r.max.x + safety) &&
            pt.y > (r.min.y - safety) && pt.y < (r.max.y + safety))
        {
            return i;
        }
    }
    LOGONCE(err2) << "Rectangle not found (pt = " << pt << ").";
    return 0;
}

//! Copies a rectangle from one atlas to another (src -> dst).
//!
void copyRect(const imgproc::UVRect &rect, const cv::Mat &src, cv::Mat &dst)
{
    for (int y = 0; y < rect.height(); y++)
    for (int x = 0; x < rect.width(); x++)
    {
        int sx(rect.x() + x), sy(rect.y() + y);
        clampMatPos(sx, sy, src);

        int dx(rect.packX + x), dy(rect.packY + y);
        if (!validMatPos(dx, dy, dst)) continue;

        dst.at<cv::Vec3b>(dy, dx) = src.at<cv::Vec3b>(sy, sx);
    }
}

//! Returns a trasformation of the (2x larger) fallback tile so that its given
//! quadrant aligns with tile of size `tileSize`.
//!
math::Matrix4 quadrantTransform(const math::Size2f &tileSize, int fallbackQuad)
{
    math::Matrix4 trafo(ublas::identity_matrix<double>(4));
    math::Size2f shift(tileSize.width / 2.0, tileSize.height / 2.0);

    switch (fallbackQuad) {
    case 0: trafo(0,3) = +shift.width, trafo(1,3) = +shift.height; break;
    case 1: trafo(0,3) = -shift.width, trafo(1,3) = +shift.height; break;
    case 2: trafo(0,3) = +shift.width, trafo(1,3) = -shift.height; break;
    case 3: trafo(0,3) = -shift.width, trafo(1,3) = -shift.height; break;
    }
    return trafo;
}

math::Matrix4 tileTransform(const math::Size2f &dstTileSize
                            , const math::Point2 dst
                            , const math::Size2f &srcTileSize
                            , const math::Point2 src)
{
    math::Matrix4 trafo(ublas::identity_matrix<double>(4));

    math::Size2f shiftSrc(srcTileSize.width / 2.0, srcTileSize.height / 2.0);
    math::Size2f shiftDst(dstTileSize.width / 2.0, dstTileSize.height / 2.0);

    math::Point2 quadrantShift(src - dst);

    trafo(0,3) = shiftSrc.width+quadrantShift(0)-shiftDst.width;
    trafo(1,3) = shiftSrc.height+quadrantShift(1)-shiftDst.height;

    return trafo;
}

MergeInput::list sortMergeInput(const MergeInput::list &mergeInput
                                , const math::Size2f &tileSize)
{
    /** Sorts via coarseness. Geometry-based coarsness is computed in lazy way.
     */
    struct TileSortInfo {
        std::size_t id;
        double coarseness;

        TileSortInfo(std::size_t id, const Tile &tile
                     , const math::Size2f &tileSize)
            : id(id), coarseness(tile.metanode.coarseness)
            , tile_(&tile), tileSize_(tileSize)
            , invGeometryCoarseness_(-1)
        {}

        double invGeometryCoarseness() const {
            if (invGeometryCoarseness_ < 0) {
                invGeometryCoarseness_
                    = tileInvGeometryCoarseness(*tile_, tileSize_);
            }
            return invGeometryCoarseness_;
        }

    private:
        const Tile *tile_;
        math::Size2f tileSize_;
        mutable double invGeometryCoarseness_;
    };

    std::vector<TileSortInfo> sortingInfos;
    bool sortinvGeometryCoarseness = false;
    for (unsigned i = 0; i < mergeInput.size(); ++i) {
        if (mergeInput[i].tile().metanode.coarseness < 0){
            sortinvGeometryCoarseness = true;
        }
        sortingInfos.emplace_back(i, mergeInput[i].tile(), tileSize);
    }

    auto compareCoarseness
        ([](const TileSortInfo &a, const TileSortInfo &b) -> bool
    {
        // tiles with lower coarseness comes last
        if (a.coarseness > b.coarseness) {
            return true;
        } else if (a.coarseness < b.coarseness) {
            return false;
        }

        // fallback if coarsenesses aren't set or are equal
        return (a.invGeometryCoarseness() < b.invGeometryCoarseness());
    });

    auto compareinvGeometryCoarseness
        ([](const TileSortInfo &a, const TileSortInfo &b) -> bool
    {
        // tiles with higher geometry quality comes last
        return (a.invGeometryCoarseness() < b.invGeometryCoarseness());
    });

    if (sortinvGeometryCoarseness){
        std::sort(sortingInfos.begin(), sortingInfos.end()
                  , compareinvGeometryCoarseness);
    } else{
        std::sort(sortingInfos.begin(), sortingInfos.end()
                  , compareCoarseness);
    }

    MergeInput::list result;
    for (const auto &tq : sortingInfos) {
        result.push_back(mergeInput[tq.id]);
    }

    return result;
}

Mesh removeDuplicateVertexes(geometry::opencv::ClipTriangle::list & faces
                             , cv::Size atlasSize)
{
    Mesh result;
    geometry::PointIndex<geometry::opencv::ClipTriangle::Point> vindex;
    geometry::PointIndex<geometry::opencv::ClipTriangle::TCoord> tindex;

    for (const auto &face1 : faces)
    {
        result.facets.emplace_back();
        Mesh::Facet &face2(result.facets.back());

        for (int i = 0; i < 3; i++) {
            face2.v[i] = vindex.assign(face1.pos[i]);
            if (unsigned(face2.v[i]) >= result.vertices.size()) {
                result.vertices.push_back(vertex(face1.pos[i]));
            }

            face2.t[i] = tindex.assign(face1.uv[i]);
            if (unsigned(face2.t[i]) >= result.texcoords.size()) {
                auto uv(normalizeUV(face1.uv[i], atlasSize));
                result.texcoords.push_back(uv);
            }
        }
    }

    return result;
}

Atlas mergeAtlases( const std::vector<Atlas*> & atlases
                  , geometry::opencv::ClipTriangle::list & faces
                    , float inflate = 0)
{
    std::vector<cv::Mat> marks;
    cv::Size safety(1, 1);

    for(uint i=0; i<atlases.size(); ++i){
        marks.emplace_back(atlases[i]->size() + safety, CV_8U, cv::Scalar(0));
    }
    for (const geometry::opencv::ClipTriangle &face : faces) {
        markFace(face, marks[face.id1]);
    }

    // determine UV rectangles that will get repacked
    std::vector<std::vector<imgproc::UVRect> > rects;
    for (const auto &m : marks)
    {
        std::vector<std::vector<cv::Point> > contours;
        findContours(m, contours);

        // add one UVRect for each connected component in the mark matrix
        rects.emplace_back();
        for (const auto& c : contours) {
            rects.back().push_back(makeRect(c));
        }

#if DEBUG
        cv::Mat dbg(m.size(), CV_8UC3, cv::Scalar(0,0,0));
        for (const auto &c : contours) {
            cv::Vec3b color(rand() % 256, rand() % 256, rand() % 256);
            for (const cv::Point &pt : c) {
                dbg.at<cv::Vec3b>(pt.y, pt.x) = color;
            }
        }
        static int idx = 0;
        auto filename(str(boost::format("merge%03d.png") % (idx++)));
        cv::imwrite(filename, dbg);
#endif
    }

    // recalculate the rectangles directly from face UVs for subpixel precision
    for (auto &face : faces) {
        face.id2 = findRect(face, rects[face.id1]);
    }
    for (auto &rlist : rects) {
        for (auto &r : rlist)
            r.clear();
    }
    for (const auto &face : faces) {
        for (int i = 0; i < 3; i++)
            rects[face.id1][face.id2].update(face.uv[i]);
    }

    //inflate rectangles
    if(inflate>0){
        for (auto &rlist : rects) {
            for (auto &r : rlist)
                r.inflate(inflate);
        }
    }

    // pack the rectangles
    imgproc::RectPacker packer;
    for (auto &rlist : rects) {
        for (auto &r : rlist)
            packer.addRect(&r);
    }
    packer.pack();

    // copy rectangles to final atlas
    cv::Mat atlas(packer.height(), packer.width(), CV_8UC3, cv::Scalar(0,0,0));
    for (unsigned i = 0; i < atlases.size(); i++) {
        for (const auto &rect : rects[i]) {
            // TODO: mask out pixels that are not needed!
            copyRect(rect, *atlases[i], atlas);
        }
    }

    // adjust face UVs to point to the new atlas
    for (auto &face : faces) {
        for (int i = 0; i < 3; i++)
            rects[face.id1][face.id2].adjustUV(face.uv[i]);
    }

    return atlas;
}

int sameIndices(const cv::Mat &qbuffer)
{
    int index = -1;
    for (auto it = qbuffer.begin<QBufferCType>();
         it != qbuffer.end<QBufferCType>(); ++it)
    {
        const auto value(*it);
        if (value < 0) { continue; }
        if (value != index) {
            if (index < 0) {
                index = value;
            } else {
                return -1;
            }
        }
    }
    return index;
}

//! Returns true if 'qbuffer' contains at least one element equal to 'index'.
//!
bool haveIndices(const cv::Mat &qbuffer, int index)
{
    for (auto it = qbuffer.begin<QBufferCType>();
         it != qbuffer.end<QBufferCType>(); ++it)
    {
        if (*it == index) return true;
    }
    return false;
}

//! Bilinearly upsamples the specified quadrant of the fallback tile heightmap.
//!
void getFallbackHeightmap(const Tile &fallback, int fallbackQuad,
                          float heightmap[MetaNode::HMSize][MetaNode::HMSize])
{
    const int hms(MetaNode::HMSize);

    double x(0.), y(0.);
    if (fallbackQuad & 1) x += double(hms - 1)*0.5;
    if (fallbackQuad & 2) y += double(hms - 1)*0.5;

    for (int i = 0; i < hms; i++)
    for (int j = 0; j < hms; j++)
    {
        math::Point2 point(x + 0.5*j, y + 0.5*i);
        heightmap[i][j] = 0;
        imgproc::bilinearInterpolate(
                   (float*) fallback.metanode.heightmap, hms, hms, hms, 1,
                   point, &heightmap[i][j]);
    }
}

MergeInput clipQuad( const MergeInput & mergeInput, int fallbackQuad
                     , const math::Size2f &tileSize)
{

    math::Matrix4 shift = quadrantTransform(tileSize, fallbackQuad);
    const Tile &tile(mergeInput.tile());
    const Mesh &mesh(tile.mesh);
    cv::Size asize(tile.atlas.size());
    asize.width *= 2;
    asize.height *= 2;

    geometry::opencv::ClipTriangle::list faces;
    for (unsigned j = 0; j < mesh.facets.size(); j++) {
        const Mesh::Facet &face(mesh.facets[j]);
        faces.emplace_back(0,0,
                vertex(transform(shift,mesh.vertices[face.v[0]])),
                vertex(transform(shift,mesh.vertices[face.v[1]])),
                vertex(transform(shift,mesh.vertices[face.v[2]])),
                denormalizeUV(mesh.texcoords[face.t[0]], asize),
                denormalizeUV(mesh.texcoords[face.t[1]], asize),
                denormalizeUV(mesh.texcoords[face.t[2]], asize));
    }
    clipFaces(faces, tileSize);

    Atlas atlas(asize.height, asize.width, tile.atlas.type());
    cv::resize( tile.atlas, atlas, atlas.size()
              , 0, 0, cv::INTER_LANCZOS4);

    std::vector<Atlas*> usedAtlases;
    usedAtlases.push_back(&atlas);

    Tile resultTile;
    resultTile.atlas = mergeAtlases(usedAtlases, faces, 2);
    resultTile.mesh = removeDuplicateVertexes(faces, resultTile.atlas.size());
    resultTile.metanode = tile.metanode;
    getFallbackHeightmap(tile, fallbackQuad, resultTile.metanode.heightmap);

    return { resultTile, &mergeInput.tileSet()
           , mergeInput.tileId(), mergeInput.srcFaceCount() };
}

} // namespace

//! Merges several tiles. The process has four phases:
//!
//! 1. AncestorTiles are clipped and transformed according to 'quad'.
//!    The meshes of all tiles are rasterized into a buffer which determines
//!    the source of geometry at each point of the result. The tiles are
//!    rasterized from worst to best (in terms of their geometry quality).
//!    This means that the best tile will be complete in the result and only
//!    the remaining space will be filled with other tiles.
//!
//! 2. Faces that will appear in the output are collected, according to the
//!    contents of the buffer. A face from a tile is used if at least one
//!    underlying element of the qbuffer indicates that the tile should be used
//!    at that place.
//!
//! 3. Pixels in atlases that are needed by the collected faces are marked.
//!    Rectangles that cover the marked areas are identified and then repacked
//!    into a single new atlas.
//!
//! 4. The isolated faces are converted into a standard mesh with vertices and
//!    texture vertices that don't repeat.
//!
MergedTile merge( const TileId &tileId, const math::Size2f &tileSize
                , const MergeInput::list &mergeInput
                , int quad
                , const MergeInput::list &ancestorTiles
                , MergeInput::list &incidentTiles)
{
    LOG(info2)<<"Merging tile "<<tileId<<" from "<<mergeInput.size()<<" sets";

    // sort tiles by quality
    auto sortedMergeInput = sortMergeInput(mergeInput, tileSize);
    // create qbuffer, rasterize meshes in increasing order of quality
    const int QBSize(512);
    cv::Mat qbuffer(QBSize, QBSize, QBufferCvType, cv::Scalar(-1));
    math::Matrix4 trafo(tileToBuffer(tileSize, QBSize));

    std::set<const TileSet*> mergedTileSetMap;
    for (const auto &mi : sortedMergeInput) {
        mergedTileSetMap.insert(&mi.tileSet());
    }

    incidentTiles.clear();
    //clip and repack ancestor tiles
    for (const auto &fb : ancestorTiles){
        if (mergedTileSetMap.find(&fb.tileSet()) == mergedTileSetMap.end()) {
            MergeInput mi = clipQuad(fb, quad, tileSize);
            if (!mi.tile().mesh.facets.empty()) {
                incidentTiles.push_back(mi);
            }
        }
    }
    incidentTiles.insert(incidentTiles.end(), sortedMergeInput.begin()
                         , sortedMergeInput.end());

    for (uint i = 0; i<incidentTiles.size(); ++i) {
        rasterizeTile(incidentTiles[i].tile(), trafo, i, qbuffer);
    }

    // close small areas of bad stuff with good stuff
    close<1, 6>(qbuffer);

    int index(sameIndices(qbuffer));
    if (index > -1) {
        return { incidentTiles[index].tile(), incidentTiles[index].tileSet() };
    }

    std::vector<Atlas*> usedAtlases;
    geometry::opencv::ClipTriangle::list mergedFaces;
    const int hms(MetaNode::HMSize);
    float minGsd = std::numeric_limits<float>::max();
    float heightmap[hms][hms];
    cv::Mat hmask(hms, hms, CV_32S, cv::Scalar(0));

    // result tile
    MergedTile result;

    for(int i=incidentTiles.size() - 1; i>=0; --i) {
        if(haveIndices(qbuffer,i)){

            const Tile &tile(incidentTiles[i].tile());
            cv::Size asize(tile.atlas.size());

            geometry::Obj mesh = tile.mesh;

            //refine mesh if necessary
            if (incidentTiles[i].srcFaceCount() > tile.mesh.facets.size()) {
                // tile has been clipped -> refine
                auto cmesh = *geometry::asMesh(mesh);
                auto rmesh(geometry::refine( cmesh
                                       , incidentTiles[i].srcFaceCount()));
                mesh = geometry::asObj(rmesh);
            }

            usedAtlases.emplace_back(&incidentTiles[i].tile().atlas);
            for (unsigned j = 0; j < mesh.facets.size(); j++) {
                const Mesh::Facet &face(mesh.facets[j]);
                if (faceCovered(mesh, face, trafo, i, qbuffer)) {
                    mergedFaces.emplace_back(usedAtlases.size()-1,0,
                            vertex(mesh.vertices[face.v[0]]),
                            vertex(mesh.vertices[face.v[1]]),
                            vertex(mesh.vertices[face.v[2]]),
                            denormalizeUV(mesh.texcoords[face.t[0]], asize),
                            denormalizeUV(mesh.texcoords[face.t[1]], asize),
                            denormalizeUV(mesh.texcoords[face.t[2]], asize) );
                }
            }

            //update metadata
            minGsd = std::min(tile.metanode.gsd, minGsd);
            for (int c = 0; c < hms; c++)
            for (int r = 0; r < hms; r++)
            {
                int what = qbuffer.at<QBufferCType>( c * (QBSize-1) / hms
                                                     , r * (QBSize-1) / hms);
                if(what==i || hmask.at<int>(c,r)==0){
                    hmask.at<int>(c,r)=1;
                    heightmap[c][r] = tile.metanode.heightmap[c][r];
                }
            }

            // remember tile's set as origin of this tile
            result.sources.push_back(&incidentTiles[i].tileSet());
        }
    }

    // almost done
    result.atlas = mergeAtlases(usedAtlases, mergedFaces);
    result.mesh = removeDuplicateVertexes(mergedFaces, result.atlas.size());

    if(sortedMergeInput.size()>0){
        result.metanode = sortedMergeInput.front().tile().metanode;
    }

    result.metanode.gsd = minGsd;
    //coarseness is not used after merge - set to invalid value
    result.metanode.coarseness = -1;

    // copy heightmap
    std::copy(&heightmap[0][0], &heightmap[hms - 1][hms]
              , &result.metanode.heightmap[0][0]);

    return result;
}

boost::optional<double> MergedTile::pixelSize() const
{
    // pixel size is valid only when tile is from single source and has valid
    // pixelsize
    if (singleSource() && metanode.exists()) {
        return metanode.pixelSize[0][0];
    }
    return boost::none;
}

} } // namespace vtslibs::vts0