// ====================================================================
// This file is part of FlexibleSUSY.
//
// FlexibleSUSY is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published
// by the Free Software Foundation, either version 3 of the License,
// or (at your option) any later version.
//
// FlexibleSUSY is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with FlexibleSUSY.  If not, see
// <http://www.gnu.org/licenses/>.
// ====================================================================

// File generated at Sat 27 Aug 2016 12:43:04

#include "SingletDM_two_scale_initial_guesser.hpp"
#include "SingletDM_two_scale_model.hpp"
#include "lowe.h"
#include "error.hpp"
#include "ew_input.hpp"
#include "wrappers.hpp"

#include <Eigen/Core>
#include <cassert>

namespace flexiblesusy {

#define DERIVEDPARAMETER(p) model->p()
#define INPUTPARAMETER(p) model->get_input().p
#define MODELPARAMETER(p) model->get_##p()
#define PHASE(p) model->get_##p()
#define LowEnergyConstant(p) Electroweak_constants::p
#define MODEL model

SingletDM_initial_guesser<Two_scale>::SingletDM_initial_guesser(
   SingletDM<Two_scale>* model_,
   const softsusy::QedQcd& qedqcd_,
   const SingletDM_low_scale_constraint<Two_scale>& low_constraint_,
   const SingletDM_susy_scale_constraint<Two_scale>& susy_constraint_,
   const SingletDM_high_scale_constraint<Two_scale>& high_constraint_
)
   : Initial_guesser<Two_scale>()
   , model(model_)
   , qedqcd(qedqcd_)
   , mu_guess(0.)
   , mc_guess(0.)
   , mt_guess(0.)
   , md_guess(0.)
   , ms_guess(0.)
   , mb_guess(0.)
   , me_guess(0.)
   , mm_guess(0.)
   , mtau_guess(0.)
   , running_precision(1.0e-3)
   , low_constraint(low_constraint_)
   , susy_constraint(susy_constraint_)
   , high_constraint(high_constraint_)
{
   assert(model && "SingletDM_initial_guesser: Error: pointer to model"
          " SingletDM<Two_scale> must not be zero");
}

SingletDM_initial_guesser<Two_scale>::~SingletDM_initial_guesser()
{
}

/**
 * Guesses the DR-bar model parameters by calling
 * guess_susy_parameters() and guess_soft_parameters() .
 */
void SingletDM_initial_guesser<Two_scale>::guess()
{
   guess_susy_parameters();
   guess_soft_parameters();
}

/**
 * Guesses the SUSY parameters (gauge, Yukawa couplings) at
 * \f$m_\text{top}^\text{pole}\f$ from the Standard Model gauge
 * couplings and fermion masses.  Threshold corrections are ignored.
 * The user-defined initial guess at the low-scale
 * (InitialGuessAtLowScale) is applied here:
 *
 * \code{.cpp}
   const auto LamSHInput = INPUTPARAMETER(LamSHInput);
   const auto LamSInput = INPUTPARAMETER(LamSInput);
   const auto muSInput = INPUTPARAMETER(muSInput);
   const auto HiggsIN = INPUTPARAMETER(HiggsIN);

   MODEL->set_v(Re(LowEnergyConstant(vev)));
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
   MODEL->set_LamSH(Re(LamSHInput));
   MODEL->set_LamS(Re(LamSInput));
   MODEL->set_muS(Re(muSInput));
   MODEL->set_muH(Re(HiggsIN));

 * \endcode
 */
void SingletDM_initial_guesser<Two_scale>::guess_susy_parameters()
{
   using namespace softsusy;

   softsusy::QedQcd leAtMt(qedqcd);
   const double MZ = Electroweak_constants::MZ;
   const double MW = Electroweak_constants::MW;
   const double sinThetaW2 = 1.0 - Sqr(MW / MZ);
   const double mtpole = leAtMt.displayPoleMt();

   mu_guess = leAtMt.displayMass(mUp);
   mc_guess = leAtMt.displayMass(mCharm);
   mt_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(mTop) - 30.0 :
      leAtMt.displayPoleMt();
   md_guess = leAtMt.displayMass(mDown);
   ms_guess = leAtMt.displayMass(mStrange);
   mb_guess = leAtMt.displayMass(mBottom);
   me_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(mElectron) :
      leAtMt.displayPoleMel();
   mm_guess = model->get_thresholds() > 0 ?
      leAtMt.displayMass(mMuon) :
      leAtMt.displayPoleMmuon();
   mtau_guess = leAtMt.displayMass(mTau);

   // guess gauge couplings at mt
   const DoubleVector alpha_sm(leAtMt.getGaugeMu(mtpole, sinThetaW2));

   MODEL->set_g1(Sqrt(4. * Pi * alpha_sm(1)));
   MODEL->set_g2(Sqrt(4. * Pi * alpha_sm(2)));
   MODEL->set_g3(Sqrt(4. * Pi * alpha_sm(3)));

   model->set_scale(mtpole);

   // apply user-defined initial guess at the low scale
   const auto LamSHInput = INPUTPARAMETER(LamSHInput);
   const auto LamSInput = INPUTPARAMETER(LamSInput);
   const auto muSInput = INPUTPARAMETER(muSInput);
   const auto HiggsIN = INPUTPARAMETER(HiggsIN);

   MODEL->set_v(Re(LowEnergyConstant(vev)));
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
   MODEL->set_LamSH(Re(LamSHInput));
   MODEL->set_LamS(Re(LamSInput));
   MODEL->set_muS(Re(muSInput));
   MODEL->set_muH(Re(HiggsIN));

}

void SingletDM_initial_guesser<Two_scale>::calculate_DRbar_yukawa_couplings()
{
   calculate_Yu_DRbar();
   calculate_Yd_DRbar();
   calculate_Ye_DRbar();
}

/**
 * Calculates the Yukawa couplings Yu of the up-type quarks
 * from the Standard Model up-type quark masses (ignoring threshold
 * corrections).
 */
void SingletDM_initial_guesser<Two_scale>::calculate_Yu_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> upQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   upQuarksDRbar(0,0) = mu_guess;
   upQuarksDRbar(1,1) = mc_guess;
   upQuarksDRbar(2,2) = mt_guess;

   const auto v = MODELPARAMETER(v);
   MODEL->set_Yu((-((1.4142135623730951*upQuarksDRbar)/v).transpose()).real());

}

/**
 * Calculates the Yukawa couplings Yd of the down-type
 * quarks from the Standard Model down-type quark masses (ignoring
 * threshold corrections).
 */
void SingletDM_initial_guesser<Two_scale>::calculate_Yd_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downQuarksDRbar(ZEROMATRIXCOMPLEX(3,3));
   downQuarksDRbar(0,0) = md_guess;
   downQuarksDRbar(1,1) = ms_guess;
   downQuarksDRbar(2,2) = mb_guess;

   const auto v = MODELPARAMETER(v);
   MODEL->set_Yd((((1.4142135623730951*downQuarksDRbar)/v).transpose()).real())
      ;

}

/**
 * Calculates the Yukawa couplings Ye of the leptons
 * from the Standard Model down-type lepton masses (ignoring threshold
 * corrections).
 */
void SingletDM_initial_guesser<Two_scale>::calculate_Ye_DRbar()
{
   Eigen::Matrix<std::complex<double>,3,3> downLeptonsDRbar(ZEROMATRIXCOMPLEX(3,3));
   downLeptonsDRbar(0,0) = me_guess;
   downLeptonsDRbar(1,1) = mm_guess;
   downLeptonsDRbar(2,2) = mtau_guess;

   const auto v = MODELPARAMETER(v);
   MODEL->set_Ye((((1.4142135623730951*downLeptonsDRbar)/v).transpose()).real()
      );

}

/**
 * Guesses the soft-breaking parameters.  At first it runs to the
 * guess of the high-scale (HighScaleFirstGuess) and imposes the
 * high-scale constraint (HighScaleInput):
 *
 * \code{.cpp}

 * \endcode
 *
 * Afterwards, it runs to the low-scale guess (LowScaleFirstGuess) and
 * solves the EWSB conditions at the tree-level.  Finally the DR-bar
 * mass spectrum is calculated.
 */
void SingletDM_initial_guesser<Two_scale>::guess_soft_parameters()
{
   const double low_scale_guess = low_constraint.get_initial_scale_guess();
   const double high_scale_guess = high_constraint.get_initial_scale_guess();

   model->run_to(high_scale_guess, running_precision);

   // apply high-scale constraint
   high_constraint.set_model(model);
   high_constraint.apply();

   // apply user-defined initial guess at the high scale

   model->run_to(low_scale_guess, running_precision);

   // apply EWSB constraint
   model->solve_ewsb_tree_level();

   // calculate tree-level spectrum
   model->calculate_DRbar_masses();
}

} // namespace flexiblesusy


#include "CoverageStats.h"
using reseq::CoverageStats;

//include <algorithm>
using std::min;
using std::max;
using std::sort;
//include <array>
using std::array;
//include <cmath>
using std::ceil;
using std::exp;
using std::pow;
using std::round;
#include <limits>
using std::numeric_limits;
//include <mutex>
using std::mutex;
using std::lock_guard;
//include <utility>
using std::pair;

#include <boost/math/distributions/poisson.hpp>
using boost::math::cdf;
using boost::math::poisson_distribution;

//include <seqan/bam_io.h>
using seqan::Dna;
using seqan::Dna5;
using seqan::hasFlagRC;
using seqan::hasFlagLast;

//include "utilities.hpp"
using reseq::utilities::ConstDna5StringReverseComplement;
using reseq::utilities::ConstIupacStringReverseComplement;
using reseq::utilities::ReversedConstCharString;
using reseq::utilities::ReversedConstCigarString;
using reseq::utilities::Complement;
using reseq::utilities::at;
using reseq::utilities::Divide;
using reseq::utilities::DominantBase;
using reseq::utilities::IsN;
using reseq::utilities::Percent;
using reseq::utilities::SafePercent;
using reseq::utilities::SetToMax;
using reseq::utilities::SetToMin;
using reseq::utilities::TransformDistanceToStartOfErrorRegion;

// Definitions so that referencing a const static is valid
const reseq::uintCovCount reseq::CoverageStats::kMaxCoverage;

void CoverageStats::EvalRead( FullRecord *record, CoverageStats::CoverageBlock *coverage_block, const Reference &reference, QualityStats &qualities, ErrorStats &errors, uintQual phred_quality_offset){
	// Fragments that did not made the filters have to_ref_pos_=0
	if(record->to_ref_pos_){
		ConstIupacStringReverseComplement reversed_seq(record->record_.seq);
		ReversedConstCharString reversed_qual(record->record_.qual);
		ReversedConstCigarString rev_cigar(record->record_.cigar);

		intSeqShift coverage_pos; // Can be before the start of the block, so negative values need to be allowed
		uintSeqLen ref_pos;
		if( hasFlagRC(record->record_) ){
			coverage_pos = record->to_ref_pos_-coverage_block->start_pos_-1;
			ref_pos = record->to_ref_pos_-1;
		}
		else{
			coverage_pos = record->from_ref_pos_-coverage_block->start_pos_;
			ref_pos = record->from_ref_pos_;
		}

		uintReadLen read_pos(0), not_considered_ref_bases(0);
		Dna ref_base;
		Dna5 base, dom_error;
		uintQual qual(0), last_qual(1);
		uintReadLen num_errors = 0;
		uintPercent error_rate;
		uintReadLenCalc error_rate_sum(0);
		for( const auto &cigar_element : (hasFlagRC(record->record_)?rev_cigar:record->record_.cigar) ){
			switch( cigar_element.operation ){
			case 'M':
			case '=':
			case 'X':
			case 'S': // Treat soft-clipping as match, so that bwa and bowtie2 behave the same
				for(auto i=cigar_element.count; i--; ){
					if(record->from_ref_pos_ <= ref_pos && ref_pos < record->to_ref_pos_){
						// We are currently not comparing the adapter to the reference
						if( hasFlagRC(record->record_) ){
							base = at(reversed_seq, read_pos);
							qual = at(reversed_qual, read_pos)-phred_quality_offset;
							ref_base = Complement::Dna5(at(reference.ReferenceSequence(record->record_.rID), ref_pos));
						}
						else{
							base = at(record->record_.seq, read_pos);
							qual = at(record->record_.qual, read_pos)-phred_quality_offset;
							ref_base = at(reference.ReferenceSequence(record->record_.rID), ref_pos);
						}

						if( !CoveragePosValid(coverage_pos, coverage_block) ){
							++(not_considered_ref_bases);
						}
						else{
							if(0 > coverage_pos){
								error_rate = coverage_block->previous_coverage_.at(-1*coverage_pos-1).error_rate_.at(hasFlagRC(record->record_));
								dom_error = coverage_block->previous_coverage_.at(-1*coverage_pos-1).dom_error_.at(hasFlagRC(record->record_));
							}
							else{
								error_rate = coverage_block->coverage_.at(coverage_pos).error_rate_.at(hasFlagRC(record->record_));
								dom_error = coverage_block->coverage_.at(coverage_pos).dom_error_.at(hasFlagRC(record->record_));
							}

							qualities.AddRefBase(hasFlagLast(record->record_), ref_base, dom_error, record->tile_id_, qual, error_rate, last_qual, record->sequence_quality_, read_pos);
							errors.AddBase(hasFlagLast(record->record_), ref_base, dom_error, record->tile_id_, base, qual, read_pos, num_errors, error_rate);

							error_rate_sum += error_rate;

							if( ref_base != base ){
								++num_errors;
							}
						}

						last_qual = qual;

						++read_pos;

						if( hasFlagRC(record->record_) ){
							--coverage_pos;
							--ref_pos;
						}
						else{
							++coverage_pos;
							++ref_pos;
						}
					}
				}
				break;
			case 'N':
			case 'D':
				for(auto i=cigar_element.count; i--; ){
					if(record->from_ref_pos_ <= ref_pos && ref_pos < record->to_ref_pos_){
						// We are currently not comparing the adapter to the reference
						if( hasFlagRC(record->record_) ){
							ref_base = Complement::Dna5(at(reference.ReferenceSequence(record->record_.rID), ref_pos));
						}
						else{
							ref_base = at(reference.ReferenceSequence(record->record_.rID), ref_pos);
						}

						if( !CoveragePosValid(coverage_pos, coverage_block) ){
							++(not_considered_ref_bases);
						}
						else{
							if(0 > coverage_pos){
								error_rate_sum += coverage_block->previous_coverage_.at(-1*coverage_pos-1).error_rate_.at(hasFlagRC(record->record_));
							}
							else{
								error_rate_sum += coverage_block->coverage_.at(coverage_pos).error_rate_.at(hasFlagRC(record->record_));
							}
							++num_errors;
						}

						if( hasFlagRC(record->record_) ){
							--coverage_pos;
							--ref_pos;
						}
						else{
							++coverage_pos;
							++ref_pos;
						}
					}
				}
				break;
			case 'I':
				if(record->from_ref_pos_ <= ref_pos && ref_pos < record->to_ref_pos_){
					if( hasFlagRC(record->record_) ){
						last_qual = at(reversed_qual, read_pos+cigar_element.count-1)-phred_quality_offset; // Directly fill last_qual as qual is not needed here
						ref_base = Complement::Dna5(at(reference.ReferenceSequence(record->record_.rID), ref_pos));
					}
					else{
						last_qual = at(record->record_.qual, read_pos+cigar_element.count-1)-phred_quality_offset; // Directly fill last_qual as qual is not needed here
						ref_base = at(reference.ReferenceSequence(record->record_.rID), ref_pos);
					}

					if( CoveragePosValid(coverage_pos, coverage_block) ){
						num_errors += cigar_element.count;
					}

					read_pos += cigar_element.count;
				}
				break;
			}
		}

		if(not_considered_ref_bases < record->to_ref_pos_-record->from_ref_pos_){ // At least one base was considered
			qualities.AddRefRead(hasFlagLast(record->record_), record->tile_id_, record->reference_gc_, record->sequence_quality_, utilities::Divide(error_rate_sum, static_cast<uintReadLenCalc>(record->to_ref_pos_-record->from_ref_pos_-not_considered_ref_bases)), record->fragment_length_);
			errors.AddRead(hasFlagLast(record->record_), num_errors);
		}
	}

	delete record;
}

inline void CoverageStats::ApplyZeroCoverageRegion(){
	tmp_coverage_.at( 0 ) += zero_coverage_region_;
	tmp_coverage_.at( 0 ) -= excluded_bases_;
	for( int strand=2; strand--; ){
		tmp_coverage_stranded_.at(strand).at(0) += zero_coverage_region_;
		tmp_coverage_stranded_.at(strand).at(0) -= excluded_bases_;
	}
}

double CoverageStats::GetPositionProbabilities(CoverageBlock *block, const Reference &reference, ThreadData &thread){
	// Get coverage weighted median error rate
	thread.block_coverage_.clear();
	thread.block_coverage_.resize(block->coverage_.size(), {0,0});
	thread.error_rates_sorted_.clear();

	uintNucCount total_bases(0), correct_bases(0);
	for( uintSeqLen pos = 0; pos < block->coverage_.size(); ++pos ){
		if( block->coverage_.at(pos).valid_ ){
			auto ref_base = at(reference.ReferenceSequence(block->sequence_id_), block->start_pos_ + pos);
			auto rev_base = Complement::Dna5(ref_base);
			for( int i=5; i--; ){
				thread.block_coverage_.at(pos).at(0) += block->coverage_.at(pos).coverage_forward_.at(i);
				if(block->coverage_.at(pos).coverage_forward_.at(i) > block->coverage_.at(pos).coverage_forward_.at( block->coverage_.at(pos).dom_error_.at(0) ) && i != ref_base){
					block->coverage_.at(pos).dom_error_.at(0) = i;
				}
				thread.block_coverage_.at(pos).at(1) += block->coverage_.at(pos).coverage_reverse_.at(i);
				if(block->coverage_.at(pos).coverage_reverse_.at(i) > block->coverage_.at(pos).coverage_reverse_.at( block->coverage_.at(pos).dom_error_.at(1) ) && i != rev_base){
					block->coverage_.at(pos).dom_error_.at(1) = i;
				}
			}

			total_bases += thread.block_coverage_.at(pos).at(0) + thread.block_coverage_.at(pos).at(1);
			correct_bases += block->coverage_.at(pos).coverage_forward_.at(ref_base) + block->coverage_.at(pos).coverage_reverse_.at(rev_base);

			thread.error_rates_sorted_.emplace_back(static_cast<double>(thread.block_coverage_.at(pos).at(0)-block->coverage_.at(pos).coverage_forward_.at(ref_base)) / thread.block_coverage_.at(pos).at(0), thread.block_coverage_.at(pos).at(0));
			thread.error_rates_sorted_.emplace_back(static_cast<double>(thread.block_coverage_.at(pos).at(1)-block->coverage_.at(pos).coverage_reverse_.at(rev_base)) / thread.block_coverage_.at(pos).at(1), thread.block_coverage_.at(pos).at(1));
		}
	}

	thread.non_sytematic_probability_.clear();
	thread.non_sytematic_probability_.resize(block->coverage_.size(), {1.0,1.0});
	thread.non_sytematic_probability_sorted_.clear();

	if(correct_bases == total_bases){
		return 0.0; // Nothing is systematic if we don't have errors
	}

	sort(thread.error_rates_sorted_.begin(), thread.error_rates_sorted_.end());
	uintNucCount sum_bases(0);
	uintSeqLen median_error_pos(0);
	while(sum_bases < total_bases/2){
		sum_bases += thread.error_rates_sorted_.at(median_error_pos++).second;
	}
	while( median_error_pos < thread.error_rates_sorted_.size() && thread.error_rates_sorted_.at(median_error_pos).first == 0.0 ){ // If the median error is exactly zero, get next non-zero error
		++median_error_pos;
	}
	if(median_error_pos == thread.error_rates_sorted_.size()){
		return 0.0;
	}

	double error_rate = thread.error_rates_sorted_.at(median_error_pos).first;
	SetToMin( error_rate, static_cast<double>(total_bases-correct_bases) / total_bases ); // If the mean is lower than the median use mean (should only happen if we shift median to non-zero value before)

	++tmp_block_error_rate_.at(static_cast<uintPercent>(round(error_rate*100)));
	error_rate /= 3.0; // Systematic is only for one base

	if(0.0 == error_rate){
		return 0.0; // Nothing is systematic if we have super low error rates
	}

	for( uintSeqLen pos = 0; pos < block->coverage_.size(); ++pos ){
		if( block->coverage_.at(pos).valid_ ){
			// Handle forward direction
			uintCovCount errors = block->coverage_.at(pos).coverage_forward_.at(block->coverage_.at(pos).dom_error_.at(0));
			if( 0 < thread.block_coverage_.at(pos).at(0) ){
				if(0 == errors){
					thread.non_sytematic_probability_sorted_.push_back(1.0);
				}
				else{
					poisson_distribution<> pois_dist( thread.block_coverage_.at(pos).at(0)*error_rate );
					thread.non_sytematic_probability_sorted_.push_back( 1-cdf( pois_dist, errors-1 ) ); // Probability to have at least that many errors (1-probability to have less errors)
				}
				thread.non_sytematic_probability_.at(pos).at(0) = thread.non_sytematic_probability_sorted_.back();
				++tmp_systematic_error_p_values_.at(static_cast<size_t>(thread.non_sytematic_probability_sorted_.back()*kPValueHistBins));
			}

			// Handle reverse direction
			if( 0 < thread.block_coverage_.at(pos).at(1) ){
				errors = block->coverage_.at(pos).coverage_reverse_.at(block->coverage_.at(pos).dom_error_.at(1));
				if(0 == errors){
					thread.non_sytematic_probability_sorted_.push_back(1.0);
				}
				else{
					poisson_distribution<> pois_dist( thread.block_coverage_.at(pos).at(1)*error_rate );
					thread.non_sytematic_probability_sorted_.push_back( 1-cdf( pois_dist, errors-1 ) ); // Probability to have at least that many errors (1-probability to have less errors)
				}
				thread.non_sytematic_probability_.at(pos).at(1) = thread.non_sytematic_probability_sorted_.back();
				++tmp_systematic_error_p_values_.at(static_cast<size_t>(thread.non_sytematic_probability_sorted_.back()*kPValueHistBins));
			}
		}
	}

	// Bejamini-Hochberg
	sort(thread.non_sytematic_probability_sorted_.begin(), thread.non_sytematic_probability_sorted_.end());
	size_t last_systematic(thread.non_sytematic_probability_sorted_.size());
	while(--last_systematic && thread.non_sytematic_probability_sorted_.at(last_systematic) > kSystematicErrorFDR*(last_systematic+1)/thread.non_sytematic_probability_sorted_.size() );

	if( 0==last_systematic && thread.non_sytematic_probability_sorted_.at(0) > kSystematicErrorFDR/thread.non_sytematic_probability_sorted_.size() ){
		++tmp_block_percent_systematic_.at(0);
		return 0.0; // No index has a probability lower than expected, so seems like we don't have systematic errors
	}
	else if( last_systematic == thread.non_sytematic_probability_sorted_.size()-1 ){
		++tmp_block_percent_systematic_.at(100);
		return 1.0; // Final probability is lower than expected, so seems like we only have systematic errors;
	}
	else{
		++tmp_block_percent_systematic_.at(Percent(last_systematic+1, thread.non_sytematic_probability_sorted_.size()));
		return thread.non_sytematic_probability_sorted_.at(last_systematic);
	}
}

void CoverageStats::UpdateCoverageAtSinglePosition(CoveragePosition &nuc_coverage, array<uintCovCount, 2> &coverage, Dna5 ref_base){
	Dna5 reverse_base = Complement::Dna5(ref_base);

	if( coverage_threshold_ > coverage.at(0) ){
		nuc_coverage.coverage_sufficient_.at(0) = false;
	}
	if( coverage_threshold_ > coverage.at(1) ){
		nuc_coverage.coverage_sufficient_.at(1) = false;
	}

	auto errors_forward = coverage.at(0) - nuc_coverage.coverage_forward_.at(ref_base);
	auto errors_reverse = coverage.at(1) - nuc_coverage.coverage_reverse_.at(reverse_base);

	auto cov_sum = coverage.at(0) + coverage.at(1);
	auto error_sum = errors_forward + errors_reverse;

	// Account for coverage and error_coverage at that position
	++tmp_coverage_.at( min(cov_sum, kMaxCoverage) );
	++tmp_coverage_stranded_.at(0).at( min(coverage.at(0), kMaxCoverage) );
	++tmp_coverage_stranded_.at(1).at( min(coverage.at(1), kMaxCoverage) );
	if(cov_sum){
		++tmp_coverage_stranded_percent_.at(0).at( Percent(coverage.at(0), cov_sum) );
		++tmp_coverage_stranded_percent_.at(1).at( Percent(coverage.at(1), cov_sum) );

		if(nuc_coverage.valid_){
			++tmp_error_coverage_.at( min(error_sum, kMaxCoverage) );
			++tmp_error_coverage_percent_.at( Percent(error_sum, cov_sum) );
			if(coverage.at(0) && coverage.at(1)){
				++tmp_error_coverage_percent_stranded_.at( Percent(errors_forward, coverage.at(0)) ).at( Percent(errors_reverse, coverage.at(1)) );
			}
		}

		if( 10 <= cov_sum ){
			++tmp_coverage_stranded_percent_min_cov_10_.at(0).at( Percent(coverage.at(0), cov_sum) );
			++tmp_coverage_stranded_percent_min_cov_10_.at(1).at( Percent(coverage.at(1), cov_sum) );
			if(nuc_coverage.valid_){
				++tmp_error_coverage_percent_min_cov_10_.at( Percent(error_sum, cov_sum) );
			}

			if( 20 <= cov_sum ){
				++tmp_coverage_stranded_percent_min_cov_20_.at(0).at( Percent(coverage.at(0), cov_sum) );
				++tmp_coverage_stranded_percent_min_cov_20_.at(1).at( Percent(coverage.at(1), cov_sum) );
				if(nuc_coverage.valid_){
					++tmp_error_coverage_percent_min_cov_20_.at( Percent(error_sum, cov_sum) );

					if(10 <= coverage.at(0) && 10 <= coverage.at(1)){
						++tmp_error_coverage_percent_stranded_min_strand_cov_10_.at( Percent(errors_forward, coverage.at(0)) ).at( Percent(errors_reverse, coverage.at(1)) );
						if(20 <= coverage.at(0) && 20 <= coverage.at(1)){
							++tmp_error_coverage_percent_stranded_min_strand_cov_20_.at( Percent(errors_forward, coverage.at(0)) ).at( Percent(errors_reverse, coverage.at(1)) );
						}
					}
				}
			}
		}
	}
}

void CoverageStats::UpdateDistances(uintSeqLen &distance_to_start_of_error_region, uintPercent &start_rate, uintPercent error_rate) const{
	if( distance_to_start_of_error_region ){
		if(start_rate < error_rate){
			distance_to_start_of_error_region = 0;
			start_rate = error_rate;
		}
		else if( ++distance_to_start_of_error_region >= reset_distance_ ){
			distance_to_start_of_error_region = 0;
			start_rate = 0;
		}
	}
	else{
		if(error_rate){ // If we assign an error rate it is systematic
			distance_to_start_of_error_region = 1;
			start_rate = error_rate;
		}
	}
}

CoverageStats::CoverageBlock *CoverageStats::CreateBlock(uintRefSeqId seq_id, uintSeqLen start_pos){
	CoverageBlock *new_block;
	if( reuse_mutex_.try_lock() ){
		if( reusable_blocks_.size() ){
			new_block = reusable_blocks_.back();
			reusable_blocks_.pop_back();
			reuse_mutex_.unlock();

			new_block->sequence_id_ = seq_id;
			new_block->start_pos_ = start_pos;
			new_block->previous_block_ = last_block_;
			new_block->next_block_ = NULL;
			new_block->coverage_.clear();
			new_block->previous_coverage_.clear();
			new_block->reads_.clear();
			new_block->scheduled_for_processing_.clear();
			new_block->processed_ = false;
		}
		else{
			reuse_mutex_.unlock();
			new_block = new CoverageBlock(seq_id, start_pos, last_block_);
			new_block->previous_coverage_.reserve(maximum_read_length_on_reference_);
		}
	}
	else{
		new_block = new CoverageBlock(seq_id, start_pos, last_block_);
		new_block->previous_coverage_.reserve(maximum_read_length_on_reference_);
	}

	new_block->reads_.reserve( (*last_block_).reads_.capacity() );
	new_block->coverage_.resize( kBlockSize );
	new_block->first_variant_id_ = (*last_block_).first_variant_id_;
	(*last_block_).next_block_ = new_block;

	return new_block;
}

void CoverageStats::UpdateFirstVariant(CoverageBlock &block, const Reference &reference){
	if( reference.VariantPositionsLoaded() ){
		while( block.first_variant_id_ < reference.VariantPositions(block.sequence_id_).size() && reference.VariantPositions(block.sequence_id_).at(block.first_variant_id_) < (*last_block_).start_pos_ ){
			++block.first_variant_id_;
		}
	}
}

bool CoverageStats::IsVariantPosition( intVariantId &cur_var, uintRefSeqId ref_seq_id, uintSeqLen pos, const Reference &reference ) const{
	if( reference.VariantPositionsLoaded() ){
		while(cur_var < reference.VariantPositions(ref_seq_id).size() && reference.VariantPositions(ref_seq_id).at(cur_var) < pos){
			++cur_var;
		}

		return cur_var < reference.VariantPositions(ref_seq_id).size() && reference.VariantPositions(ref_seq_id).at(cur_var) == pos;
	}

	return false;
}

void CoverageStats::InitBlock(CoverageBlock &block, const Reference &reference){
	// Ensure that the block is not reaching over the end of the reference sequence
	if( reference.SequenceLength(block.sequence_id_) < block.start_pos_+kBlockSize){
		block.coverage_.resize( reference.SequenceLength(block.sequence_id_)-block.start_pos_ );
	}

	// Check for invalid positions
	auto cur_var = block.first_variant_id_;
	for(auto pos = block.start_pos_; pos < block.start_pos_+block.coverage_.size(); ++pos){
		if( IsN( at(reference.ReferenceSequence(block.sequence_id_), pos) ) || IsVariantPosition( cur_var, block.sequence_id_, pos, reference  ) ){
			block.coverage_.at(pos-block.start_pos_).valid_ = false;
		}
	}
}

void CoverageStats::ProcessBlock(CoverageBlock *block, const Reference &reference, ThreadData &thread){
	auto max_probability_systematic = GetPositionProbabilities(block, reference, thread);

	for( uintSeqLen pos = 0; pos < block->coverage_.size(); ++pos ){
		// Set systematic errors and filter out variants
		array<uintCovCount, 2> errors = {block->coverage_.at(pos).coverage_forward_.at(block->coverage_.at(pos).dom_error_.at(0)), block->coverage_.at(pos).coverage_reverse_.at(block->coverage_.at(pos).dom_error_.at(1))};
		array<uintCovCount, 2> &coverage = thread.block_coverage_.at(pos);
		if(block->coverage_.at(pos).valid_){
			for(uintTempSeq strand=0; strand < 2; ++strand){
				if(coverage.at(strand)){
					if( NonSystematicError(coverage.at(strand), errors.at(strand), thread.non_sytematic_probability_.at(pos).at(strand), max_probability_systematic) ){
						block->coverage_.at(pos).dom_error_.at(strand) = 4;
					}
					else{
						block->coverage_.at(pos).error_rate_.at(strand) = Percent(errors.at(strand),coverage.at(strand));
					}
				}
			}

			if( 4 != block->coverage_.at(pos).dom_error_.at(0) && 4 != block->coverage_.at(pos).dom_error_.at(1) && block->coverage_.at(pos).dom_error_.at(0) == Complement::Dna5(block->coverage_.at(pos).dom_error_.at(1)) ){
				// Variant not in the vcf file
				block->coverage_.at(pos).valid_ = false;
			}
		}

		// Store coverage and errors
		UpdateCoverageAtSinglePosition( block->coverage_.at(pos), thread.block_coverage_.at(pos), at(reference.ReferenceSequence(block->sequence_id_), block->start_pos_ + pos) );
	}

	// Save information that is still needed in next_block_
	if( block->next_block_ && (*(block->next_block_)).sequence_id_ == block->sequence_id_ ){
		// Next block is on the same reference sequence
		uintSeqLen overlap(0);
		if( reset_distance_-1 > (*(block->next_block_)).start_pos_ - (block->start_pos_ + block->coverage_.size()) ){
			overlap = BasesWithInSysErrorResetDistance(block);
		}

		uintSeqLen overlap_start(1);
		if(block->start_pos_+kBlockSize == (*block->next_block_).start_pos_){
			// Blocks are directly connected, so reads that are shared between both potentially exist
			SetToMax(overlap, 2*maximum_read_length_on_reference_);
		}
		else{
			// No shared reads exists as blocks are not connected, only systematic errors have to be propagated taking the gap into account
			overlap_start += reset_distance_ - overlap;
		}

		for(uintReadLen pos_before = 1; pos_before < overlap_start; ++pos_before){
			(*block->next_block_).previous_coverage_.emplace_back(); // Insert dummy as we have a short empty region between the two blocks
		}
		for(uintReadLen pos_overlap = 1; pos_overlap <= overlap; ++pos_overlap){
			(*block->next_block_).previous_coverage_.emplace_back( block->coverage_.at(kBlockSize-pos_overlap) );
		}
	}

	block->processed_ = true;
}

void CoverageStats::CountBlock(CoverageBlock *block, const Reference &reference){
	// Enter error_rates_ and dominant_errors_ in forward direction
	Dna5 ref_base, prev_ref_base(4);
	DominantBase dom_ref_base;
	if(block->start_pos_){
		prev_ref_base = at(reference.ReferenceSequence(block->sequence_id_), block->start_pos_ - 1);
		dom_ref_base.Set( reference.ReferenceSequence(block->sequence_id_), block->start_pos_ );
	}
	uintSeqLen gc_bases = min(block->start_pos_, gc_range_);
	uintSeqLen n_count(0);
	uintSeqLen gc = reference.GCContentAbsolut( n_count, block->sequence_id_, block->start_pos_-gc_bases, block->start_pos_);

	// Initialize distance and start_rate
	uintSeqLen distance_to_start_of_error_region_forward(0);
	uintPercent start_rate_forward(0);
	if( block->previous_coverage_.size() ){
		for(auto rev_pos = reset_distance_; --rev_pos; ){ // Index 0 is the one directly before this block, so correct order from low to high reference pos is reverse order in previous_coverage_
			UpdateDistances(distance_to_start_of_error_region_forward, start_rate_forward, (block->previous_coverage_.at(rev_pos).valid_ && block->previous_coverage_.at(rev_pos).coverage_sufficient_.at(0)?block->previous_coverage_.at(rev_pos).error_rate_.at(0):0));
		}
	}

	for( uintSeqLen pos = 0; pos < block->coverage_.size(); ++pos ){
		ref_base = at(reference.ReferenceSequence(block->sequence_id_), block->start_pos_ + pos);

		// Enter values
		if(block->coverage_.at(pos).valid_ && block->coverage_.at(pos).coverage_sufficient_.at(0)){
			AddSysError(ref_base, prev_ref_base, dom_ref_base.Get(), block->coverage_.at(pos).dom_error_.at(0), block->coverage_.at(pos).error_rate_.at(0), TransformDistanceToStartOfErrorRegion(distance_to_start_of_error_region_forward), SafePercent(gc,gc_bases-n_count), start_rate_forward);
		}

		// Set values for next iteration
		prev_ref_base = ref_base;
		dom_ref_base.Update( ref_base, reference.ReferenceSequence(block->sequence_id_), block->start_pos_ + pos);
		UpdateDistances(distance_to_start_of_error_region_forward, start_rate_forward, (block->coverage_.at(pos).valid_ && block->coverage_.at(pos).coverage_sufficient_.at(0)?block->coverage_.at(pos).error_rate_.at(0):0));
		auto new_pos = block->start_pos_ + pos;
		if(gc_bases<gc_range_){
			++gc_bases;
			if( reference.GC( block->sequence_id_, new_pos ) ){
				++gc;
			}
			else if( reference.N( block->sequence_id_, new_pos ) ){
				++n_count;
			}
		}
		else{
			reference.UpdateGC( gc, n_count, block->sequence_id_, new_pos - gc_bases, new_pos);
		}
	}

	// Enter error_rates_ and dominant_errors_ in reverse direction
	uintSeqLen last_pos = block->coverage_.size();
	if( NextBlockWithInSysErrorResetDistance(block) ){
		// Next block is at the same reference sequence and closer than reset_distance: Everything that is potentially in an error region starting in the next block cannot be handled yet
		last_pos -= BasesWithInSysErrorResetDistance(block);
	}
	--last_pos; // Shift it from end pos to last valid pos

	ConstDna5StringReverseComplement reversed_seq(reference.ReferenceSequence(block->sequence_id_));
	uintSeqLen ref_pos = reference.SequenceLength(block->sequence_id_) - (block->start_pos_ + last_pos) - 1;
	dom_ref_base.Clear();
	if(ref_pos){
		prev_ref_base = at(reversed_seq, ref_pos - 1);
		dom_ref_base.Set( reversed_seq, ref_pos );
	}
	else{
		prev_ref_base = 4;
	}
	gc_bases = min(ref_pos, gc_range_);
	n_count = 0;
	gc = reference.GCContentAbsolut( n_count, block->sequence_id_, block->start_pos_+last_pos + 1, block->start_pos_+last_pos + 1 + gc_bases); // + 1 because we want the bases after first one we handle (previous bases in reverse direction)

	// Initialize distance and start_rate
	uintSeqLen distance_to_start_of_error_region_reverse(0);
	uintPercent start_rate_reverse(0);
	for( uintSeqLen pos = block->coverage_.size(); pos-- > last_pos; ){
		UpdateDistances(distance_to_start_of_error_region_reverse, start_rate_reverse, (block->coverage_.at(pos).valid_ && block->coverage_.at(pos).coverage_sufficient_.at(1)?block->coverage_.at(pos).error_rate_.at(1):0));
	}

	for( uintSeqLen pos = last_pos+1; pos--; ){
		ref_base = at(reversed_seq, ref_pos);

		// Enter values
		if(block->coverage_.at(pos).valid_ && block->coverage_.at(pos).coverage_sufficient_.at(1)){
			AddSysError(ref_base, prev_ref_base, dom_ref_base.Get(), block->coverage_.at(pos).dom_error_.at(1), block->coverage_.at(pos).error_rate_.at(1), TransformDistanceToStartOfErrorRegion(distance_to_start_of_error_region_reverse), SafePercent(gc,gc_bases-n_count), start_rate_reverse);
		}

		// Set values for next iteration
		prev_ref_base = ref_base;
		dom_ref_base.Update( ref_base, reversed_seq, ref_pos);
		UpdateDistances(distance_to_start_of_error_region_reverse, start_rate_reverse, (block->coverage_.at(pos).valid_ && block->coverage_.at(pos).coverage_sufficient_.at(1)?block->coverage_.at(pos).error_rate_.at(1):0));
		auto new_pos = block->start_pos_+pos;
		if(gc_bases<gc_range_){
			++gc_bases;
			if( reference.GC( block->sequence_id_, new_pos ) ){
				++gc;
			}
			else if( reference.N( block->sequence_id_, new_pos ) ){
				++n_count;
			}
		}
		else{
			reference.UpdateGC( gc, n_count, block->sequence_id_, new_pos + gc_bases, new_pos);
		}
		ref_pos++;
	}

	// Handle values from last block that are not added yet due to the overlap based on reset_distance
	if( block->previous_coverage_.size() ){
		for(auto rev_pos = 0; rev_pos < reset_distance_; ++rev_pos){ // Index 0 is the one directly before this block, so correct order from high to low reference pos is normal order in previous_coverage_
			ref_base = at(reversed_seq, ref_pos);

			// Enter values
			if(block->previous_coverage_.at(rev_pos).valid_ && block->previous_coverage_.at(rev_pos).coverage_sufficient_.at(1)){
				AddSysError(ref_base, prev_ref_base, dom_ref_base.Get(), block->previous_coverage_.at(rev_pos).dom_error_.at(1), block->previous_coverage_.at(rev_pos).error_rate_.at(1), TransformDistanceToStartOfErrorRegion(distance_to_start_of_error_region_reverse), SafePercent(gc,gc_bases-n_count), start_rate_reverse);
			}

			// Set values for next iteration
			prev_ref_base = ref_base;
			dom_ref_base.Update( ref_base, reversed_seq, ref_pos);
			UpdateDistances(distance_to_start_of_error_region_reverse, start_rate_reverse, (block->previous_coverage_.at(rev_pos).valid_ && block->previous_coverage_.at(rev_pos).coverage_sufficient_.at(1)?block->previous_coverage_.at(rev_pos).error_rate_.at(1):0));
			auto new_pos = block->start_pos_-rev_pos-1;
			if(gc_bases<gc_range_){
				++gc_bases;
				if( reference.GC( block->sequence_id_, new_pos ) ){
					++gc;
				}
				else if( reference.N( block->sequence_id_, new_pos ) ){
					++n_count;
				}
			}
			else{
				reference.UpdateGC( gc, n_count, block->sequence_id_, new_pos + gc_bases, new_pos);
			}
			ref_pos++;
		}
	}
}

CoverageStats::CoverageBlock *CoverageStats::RemoveBlock(CoverageBlock *block){
	reusable_blocks_.push_back(block);

	return block->next_block_;
}

void CoverageStats::Prepare(uintCovCount average_coverage, uintReadLen average_read_length, uintReadLen maximum_read_length_on_reference){
	reusable_blocks_.reserve(100);

	if(coverage_threshold_ > average_coverage/4 ){
		if( 1 < average_coverage/4 ){
			coverage_threshold_ = average_coverage/4; // If half the average coverage is lower than the threshold, so we do not just take the statistics from repeat regions (the coverage per strand is already half the total coverage, so in total we have a quarter)
		}
		else{
			coverage_threshold_ = 1; // Do not set anything without having coverage
		}
	}
	gc_range_ = average_read_length/2;
	reset_distance_ = average_read_length;

	maximum_read_length_on_reference_ = maximum_read_length_on_reference;

	auto max_error_dist = TransformDistanceToStartOfErrorRegion(reset_distance_-1)+1;
	for( auto ref_base=4; ref_base--; ){
		for( auto prev_ref_base=5; prev_ref_base--; ){
			for( auto dom_base=4; dom_base--; ){
				SetDimensions( tmp_dominant_errors_by_distance_.at(ref_base).at(prev_ref_base).at(dom_base), max_error_dist, 5 );
				SetDimensions( tmp_dominant_errors_by_gc_.at(ref_base).at(prev_ref_base).at(dom_base), 101, 5 );
				SetDimensions( tmp_gc_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base), max_error_dist, 101 );
				SetDimensions( tmp_dominant_errors_by_start_rates_.at(ref_base).at(prev_ref_base).at(dom_base), 101, 5 );
				SetDimensions( tmp_start_rates_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base), max_error_dist, 101 );
				SetDimensions( tmp_start_rates_by_gc_de_.at(ref_base).at(prev_ref_base).at(dom_base), 101, 101 );
			}
		}

		for( auto dom_error=5; dom_error--; ){
			SetDimensions( tmp_error_rates_by_distance_.at(ref_base).at(dom_error), max_error_dist, 101 );
			SetDimensions( tmp_error_rates_by_gc_.at(ref_base).at(dom_error), 101, 101 );
			SetDimensions( tmp_gc_by_distance_er_.at(ref_base).at(dom_error), max_error_dist, 101 );
			SetDimensions( tmp_error_rates_by_start_rates_.at(ref_base).at(dom_error), 101, 101 );
			SetDimensions( tmp_start_rates_by_distance_er_.at(ref_base).at(dom_error), max_error_dist, 101 );
			SetDimensions( tmp_start_rates_by_gc_er_.at(ref_base).at(dom_error), 101, 101 );
		}
	}

	// Setting the size, those aren't atomics
	tmp_block_error_rate_.resize(101);
	tmp_block_percent_systematic_.resize(101);
	tmp_systematic_error_p_values_.resize(kPValueHistBins+1);

	tmp_coverage_.resize(kMaxCoverage+1);
	for( auto strand=2; strand--; ){
		tmp_coverage_stranded_.at(strand).resize(kMaxCoverage+1);
		tmp_coverage_stranded_percent_.at(strand).resize(101);
		tmp_coverage_stranded_percent_min_cov_10_.at(strand).resize(101);
		tmp_coverage_stranded_percent_min_cov_20_.at(strand).resize(101);
	}
	tmp_error_coverage_.resize(kMaxCoverage+1);
	tmp_error_coverage_percent_.resize(101);
	tmp_error_coverage_percent_min_cov_10_.resize(101);
	tmp_error_coverage_percent_min_cov_20_.resize(101);
	SetDimensions( tmp_error_coverage_percent_stranded_, 101, 101 );
	SetDimensions( tmp_error_coverage_percent_stranded_min_strand_cov_10_, 101, 101 );
	SetDimensions( tmp_error_coverage_percent_stranded_min_strand_cov_20_, 101, 101 );
}

CoverageStats::CoverageBlock *CoverageStats::FindBlock(uintRefSeqId ref_seq_id, uintSeqLen ref_pos){
	CoverageBlock *block = last_block_;
	while(block->sequence_id_ > ref_seq_id){
		block = block->previous_block_;
	}
	while(block->start_pos_ > ref_pos){
		block = block->previous_block_;
	}

	return block;
}

bool CoverageStats::EnsureSpace(uintRefSeqId ref_seq_id, uintSeqLen start_pos, uintSeqLen end_pos, FullRecord *record, Reference &reference){
	// Make sure the variation for the given reference sequence is already loaded
	if( reference.VariantPositionsLoaded() && !reference.VariantPositionsLoadedForSequence(ref_seq_id) ){
		lock_guard<mutex> lock(variant_loading_mutex_);
		if(!reference.ReadVariantPositions(ref_seq_id+1)){ // End is specified, so to get the current one +1 is needed
			return false;
		}
	}

	SetToMin(end_pos, reference.SequenceLength(ref_seq_id));
	if( last_block_ ){
		// As reads are ordered by position, we can jump over not covered regions
		if((*last_block_).sequence_id_ < ref_seq_id){
			// Account for the not covered region at the end of the last reference sequence
			CountEmptyEndOfSequence(reference);

			// Account for the not covered reference sequences in between
			for( auto seq_id = ref_seq_id; --seq_id > (*last_block_).sequence_id_ ; ){
				zero_coverage_region_ += reference.SequenceLength(seq_id);
				excluded_bases_ += reference.SumExcludedBases(seq_id);
				num_exclusion_regions_ += reference.NumExcludedRegions(seq_id);
			}

			// Account for the not covered region in the current reference sequence before the first position
			zero_coverage_region_ += start_pos;
			excluded_bases_ += reference.SumExcludedBases(ref_seq_id);
			num_exclusion_regions_ += reference.NumExcludedRegions(ref_seq_id);

			// Create first block of next reference sequence
			last_block_ = CreateBlock(ref_seq_id, start_pos);
			InitBlock(*last_block_, reference);
		}
		else if((*last_block_).start_pos_+kBlockSize < start_pos){
			// Account for the empty coverage region
			zero_coverage_region_ += start_pos - (*last_block_).start_pos_ - kBlockSize;

			// Create first block after the empty coverage region
			last_block_ = CreateBlock(ref_seq_id, start_pos);
			UpdateFirstVariant(*last_block_, reference);
			InitBlock(*last_block_, reference);
		}
	}
	else{
		// Initialize first block
		// Account for the not covered reference sequences before the first position
		for( auto seq_id = ref_seq_id; seq_id-- ; ){
			zero_coverage_region_ += reference.SequenceLength(seq_id);
			excluded_bases_ += reference.SumExcludedBases(seq_id);
			num_exclusion_regions_ += reference.NumExcludedRegions(seq_id);
		}

		// Account for the not covered region in the current reference sequence before the first position
		zero_coverage_region_ += start_pos;
		excluded_bases_ += reference.SumExcludedBases(ref_seq_id);
		num_exclusion_regions_ += reference.NumExcludedRegions(ref_seq_id);

		// Create new block
		auto new_block = new CoverageBlock(ref_seq_id, start_pos, NULL);
		new_block->coverage_.resize( kBlockSize );
		new_block->previous_coverage_.reserve(maximum_read_length_on_reference_);
		new_block->reads_.reserve( 2*kBlockSize );
		InitBlock(*new_block, reference);

		first_block_ = new_block;
		last_block_ = new_block;
	}

	// Create blocks until end_pos is reached
	while((*last_block_).start_pos_+kBlockSize < end_pos){
		last_block_ = CreateBlock((*last_block_).sequence_id_, (*last_block_).start_pos_+kBlockSize);
		UpdateFirstVariant(*last_block_, reference);
		InitBlock(*last_block_, reference);
	}

	// Add read to last block it potentially overlaps (most of the time it is last_block_, but it is not guaranteed so use FindBlock)
	auto reg_block = FindBlock(ref_seq_id, end_pos);
	reg_block->reads_.push_back(record);

	return true;
}

void CoverageStats::AddFragment( uintRefSeqId ref_seq_id, uintSeqLen ref_pos, CoverageBlock *&block ){
	if(block){
		while( block->start_pos_+kBlockSize <= ref_pos || block->sequence_id_ < ref_seq_id ){
			block = block->next_block_;
		}
	}
	else{
		block = FindBlock(ref_seq_id, ref_pos);
	}
	++(block->unprocessed_fragments_);
}

void CoverageStats::RemoveFragment( uintRefSeqId ref_seq_id, uintSeqLen ref_pos, CoverageBlock *&block, uintFragCount &processed_fragments ){
	if(block){
		if( block->start_pos_+kBlockSize <= ref_pos || block->sequence_id_ < ref_seq_id ){
			auto old_block = block;
			do{
				block = block->next_block_;
			} while( block->start_pos_+kBlockSize <= ref_pos || block->sequence_id_ < ref_seq_id );

			// Subtract processed_fragments from old_block only after finding the new block, as it potentially allows other threads to remove old_block
			old_block->unprocessed_fragments_ -= processed_fragments;
			processed_fragments = 0;
		}
		else if(block->start_pos_ > ref_pos){ // Here we don't need to compare to sequence_id as pairs with reads on different contigs are not added to unprocessed_fragments_
			// Here we don't need to care if we set unprocessed_fragments_ to zero before finding the new block, as it is before this block and must have at least one unprocessed_fragments_
			block->unprocessed_fragments_ -= processed_fragments;
			processed_fragments = 0;

			do{
				block = block->previous_block_;
			} while( block->start_pos_ > ref_pos );
		}
	}
	else{
		block = FindBlock(ref_seq_id, ref_pos);
	}

	++processed_fragments; // Store processed fragments instead of subtracting them directly so that last fragments from this batch can be removed after the CleanUp step, so at least one block remains during CleanUp
}

reseq::uintRefSeqId CoverageStats::CleanUp(uintSeqLen &still_needed_position, Reference &reference, QualityStats &qualities, ErrorStats &errors, uintQual phred_quality_offset, CoverageBlock *cov_block, uintFragCount &processed_fragments, ThreadData &thread){
	// Process all blocks that are not needed anymore
	CoverageBlock *until_block = first_block_;
	while(!until_block->unprocessed_fragments_){ // The unprocessed fragments from the current read haven't been removed yet, so end of list cannot be reached
		if( !until_block->scheduled_for_processing_.test_and_set() ){
			ProcessBlock(until_block, reference, thread);
		}

		until_block = until_block->next_block_;
	}

	// Remove pointers to blocks that are not needed anymore
	uintRefSeqId still_needed_reference_sequence(0);
	still_needed_position = 0;
	{
		lock_guard<mutex> lock(clean_up_mutex_);

		until_block = first_block_;
		while(!until_block->unprocessed_fragments_ && until_block->processed_){ // The unprocessed fragments from the current read haven't been removed yet, so end of list cannot be reached
			until_block = until_block->next_block_;
			first_block_ = until_block;
		}
		until_block = until_block->previous_block_;

		if(until_block){
			(*first_block_).previous_block_ = NULL;
			until_block->next_block_ = NULL;
			still_needed_reference_sequence = (*first_block_).sequence_id_;
			still_needed_position = (*first_block_).start_pos_;
		}
	}

	// Subtract processed_fragments from cov_block after until_block is determined, as it potentially allows to remove cov_block
	cov_block->unprocessed_fragments_ -= processed_fragments;
	processed_fragments = 0;

	// Count and remove blocks, where we removed the pointers before
	if(until_block){
		while(until_block->previous_block_){
			CountBlock(until_block, reference);
			for( auto rec : until_block->reads_){
				EvalRead(rec, until_block, reference, qualities, errors, phred_quality_offset);
			}
			until_block = until_block->previous_block_;
		}

		CountBlock(until_block, reference);
		for( auto rec : until_block->reads_){
			EvalRead(rec, until_block, reference, qualities, errors, phred_quality_offset);
		}

		lock_guard<mutex> lock(reuse_mutex_);
		do{
			until_block = RemoveBlock(until_block);
		} while(until_block);
	}

	reference.ClearVariants(still_needed_reference_sequence);

	return still_needed_reference_sequence;
}

bool CoverageStats::PreLoadVariants(Reference &reference){
	if(reference.VariantPositionsLoaded() && !reference.VariantPositionsCompletelyLoaded() && !reference.VariantPositionsLoadedForSequence((*last_block_).sequence_id_+2)){
		if( variant_loading_mutex_.try_lock() ){
			if( !reference.ReadVariantPositions((*last_block_).sequence_id_+2) ){
				variant_loading_mutex_.unlock();
				return false;
			}

			variant_loading_mutex_.unlock();
		}
	}

	return true;
}

bool CoverageStats::Finalize(const Reference &reference, QualityStats &qualities, ErrorStats &errors, uintQual phred_quality_offset, mutex &print_mutex, ThreadData &thread){
	// Remove all reusable_blocks_ as they are not needed anymore
	for( auto block : reusable_blocks_ ){
		delete block;
	}
	auto final_num_blocks = reusable_blocks_.size();
	reusable_blocks_.clear();

	if(first_block_){
		if( last_block_ ){
			// Update not covered regions after last block
			CountEmptyEndOfSequence(reference);

			for( auto seq_id = (*last_block_).sequence_id_+1; seq_id < reference.NumberSequences(); ++seq_id ){
				zero_coverage_region_ += reference.SequenceLength(seq_id);
				excluded_bases_ += reference.SumExcludedBases(seq_id);
				num_exclusion_regions_ += reference.NumExcludedRegions(seq_id);
			}

			// Update coverage of remaining blocks and delete them
			CoverageBlock *first_block = first_block_; // Use non atomic pointer for the finalization
			first_block_ = NULL;
			last_block_ = NULL;

			while(first_block->next_block_){
				++final_num_blocks;
				ProcessBlock(first_block, reference, thread);
				CountBlock(first_block, reference);
				for( auto rec : first_block->reads_){
					EvalRead(rec, first_block, reference, qualities, errors, phred_quality_offset);
				}

				first_block = first_block->next_block_;
				delete first_block->previous_block_;
			}

			++final_num_blocks;
			ProcessBlock(first_block, reference, thread);
			CountBlock(first_block, reference);
			for( auto rec : first_block->reads_){
				EvalRead(rec, first_block, reference, qualities, errors, phred_quality_offset);
			}
			delete first_block;
		}
		else{
			printErr << "first_block_ set, but last_block_ not. Something dodgy is going on here" << std::endl;
			return false;
		}
	}
	else{
		if( last_block_ ){
			printErr << "last_block_ set, but first_block_ not. Something dodgy is going on here" << std::endl;
			return false;
		}
		else{
			// There have been reads in the file (or Finalize wouldn't have been called), but none made the criteria to add coverage
			SetAllZero(reference);
		}
	}

	{
		lock_guard<mutex> lock(print_mutex);
		printInfo << "Needed to create " << final_num_blocks << " coverage blocks." << std::endl;
		auto total_size = reference.TotalSize();
		printInfo << "Excluded " << excluded_bases_ << " of " << total_size << " bases [" << static_cast<uintPercentPrint>(Percent(excluded_bases_, total_size)) << "%] in the reference due to contig ends and N's over " << num_exclusion_regions_ << " regions." << std::endl;
	}

	ApplyZeroCoverageRegion();

	for( auto ref_base=4; ref_base--; ){
		for( auto prev_ref_base=5; prev_ref_base--; ){
			for( auto dom_base=4; dom_base--; ){
				dominant_errors_by_distance_.at(ref_base).at(prev_ref_base).at(dom_base).Acquire( tmp_dominant_errors_by_distance_.at(ref_base).at(prev_ref_base).at(dom_base) );
				dominant_errors_by_gc_.at(ref_base).at(prev_ref_base).at(dom_base).Acquire( tmp_dominant_errors_by_gc_.at(ref_base).at(prev_ref_base).at(dom_base) );
				gc_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base).Acquire( tmp_gc_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base) );
				dominant_errors_by_start_rates_.at(ref_base).at(prev_ref_base).at(dom_base).Acquire( tmp_dominant_errors_by_start_rates_.at(ref_base).at(prev_ref_base).at(dom_base) );
				start_rates_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base).Acquire( tmp_start_rates_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base) );
				start_rates_by_gc_de_.at(ref_base).at(prev_ref_base).at(dom_base).Acquire( tmp_start_rates_by_gc_de_.at(ref_base).at(prev_ref_base).at(dom_base) );
			}
		}

		for( auto dom_error=5; dom_error--; ){
			error_rates_by_distance_.at(ref_base).at(dom_error).Acquire( tmp_error_rates_by_distance_.at(ref_base).at(dom_error) );
			error_rates_by_gc_.at(ref_base).at(dom_error).Acquire( tmp_error_rates_by_gc_.at(ref_base).at(dom_error) );
			gc_by_distance_er_.at(ref_base).at(dom_error).Acquire( tmp_gc_by_distance_er_.at(ref_base).at(dom_error) );
			error_rates_by_start_rates_.at(ref_base).at(dom_error).Acquire( tmp_error_rates_by_start_rates_.at(ref_base).at(dom_error) );
			start_rates_by_distance_er_.at(ref_base).at(dom_error).Acquire( tmp_start_rates_by_distance_er_.at(ref_base).at(dom_error) );
			start_rates_by_gc_er_.at(ref_base).at(dom_error).Acquire( tmp_start_rates_by_gc_er_.at(ref_base).at(dom_error) );
		}
	}

	block_error_rate_.Acquire( tmp_block_error_rate_ );
	block_percent_systematic_.Acquire( tmp_block_percent_systematic_ );
	tmp_systematic_error_p_values_.at(kPValueHistBins-1) += tmp_systematic_error_p_values_.at(kPValueHistBins); // Last bin only contains p-values of exactly 1.0, so merge it with the one before
	tmp_systematic_error_p_values_.at(kPValueHistBins) = 0;
	systematic_error_p_values_.Acquire( tmp_systematic_error_p_values_ );

	coverage_.Acquire( tmp_coverage_ );
	for( auto strand=2; strand--; ){
		coverage_stranded_.at(strand).Acquire( tmp_coverage_stranded_.at(strand) );
		coverage_stranded_percent_.at(strand).Acquire( tmp_coverage_stranded_percent_.at(strand) );
		coverage_stranded_percent_min_cov_10_.at(strand).Acquire( tmp_coverage_stranded_percent_min_cov_10_.at(strand) );
		coverage_stranded_percent_min_cov_20_.at(strand).Acquire( tmp_coverage_stranded_percent_min_cov_20_.at(strand) );
	}
	error_coverage_.Acquire( tmp_error_coverage_ );
	error_coverage_percent_.Acquire( tmp_error_coverage_percent_ );
	error_coverage_percent_min_cov_10_.Acquire( tmp_error_coverage_percent_min_cov_10_ );
	error_coverage_percent_min_cov_20_.Acquire( tmp_error_coverage_percent_min_cov_20_ );
	error_coverage_percent_stranded_.Acquire( tmp_error_coverage_percent_stranded_ );
	error_coverage_percent_stranded_min_strand_cov_10_.Acquire( tmp_error_coverage_percent_stranded_min_strand_cov_10_ );
	error_coverage_percent_stranded_min_strand_cov_20_.Acquire( tmp_error_coverage_percent_stranded_min_strand_cov_20_ );

	return true;
}

void CoverageStats::Shrink(){
	for( auto ref_base=4; ref_base--; ){
		for( auto prev_ref_base=5; prev_ref_base--; ){
			for( auto dom_base=4; dom_base--; ){
				ShrinkVect( dominant_errors_by_distance_.at(ref_base).at(prev_ref_base).at(dom_base) );
				ShrinkVect( dominant_errors_by_gc_.at(ref_base).at(prev_ref_base).at(dom_base) );
				ShrinkVect( gc_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base) );
				ShrinkVect( dominant_errors_by_start_rates_.at(ref_base).at(prev_ref_base).at(dom_base) );
				ShrinkVect( start_rates_by_distance_de_.at(ref_base).at(prev_ref_base).at(dom_base) );
				ShrinkVect( start_rates_by_gc_de_.at(ref_base).at(prev_ref_base).at(dom_base) );
			}
		}

		for( auto dom_error=5; dom_error--; ){
			ShrinkVect( error_rates_by_distance_.at(ref_base).at(dom_error) );
			ShrinkVect( error_rates_by_gc_.at(ref_base).at(dom_error) );
			ShrinkVect( gc_by_distance_er_.at(ref_base).at(dom_error) );
			ShrinkVect( error_rates_by_start_rates_.at(ref_base).at(dom_error) );
			ShrinkVect( start_rates_by_distance_er_.at(ref_base).at(dom_error) );
			ShrinkVect( start_rates_by_gc_er_.at(ref_base).at(dom_error) );
		}
	}

	ShrinkVect( error_coverage_percent_stranded_ );
	ShrinkVect( error_coverage_percent_stranded_min_strand_cov_10_ );
	ShrinkVect( error_coverage_percent_stranded_min_strand_cov_20_ );
}

void CoverageStats::PreparePlotting(){
	error_rates_by_distance_sum_.Clear();
	error_rates_by_gc_sum_.Clear();
	for( int ref_base=4; ref_base--; ){
		for( int dom_error=5; dom_error--; ){
			error_rates_by_distance_sum_ += error_rates_by_distance_.at(ref_base).at(dom_error);
			error_rates_by_gc_sum_ += error_rates_by_gc_.at(ref_base).at(dom_error);
		}
	}
}


#ifndef VEXCL_MBA_HPP
#define VEXCL_MBA_HPP

/*
The MIT License

Copyright (c) 2012-2018 Denis Demidov <dennis.demidov@gmail.com>

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.  IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/

/**
 * \file   vexcl/mba.hpp
 * \author Denis Demidov <dennis.demidov@gmail.com>
 * \brief  Scattered data interpolation with multilevel B-Splines.
 */

#include <vector>
#include <array>
#include <sstream>
#include <memory>
#include <algorithm>
#include <numeric>
#include <type_traits>
#include <cassert>

#include <boost/tuple/tuple.hpp>
#include <boost/fusion/adapted/boost_tuple.hpp>

#include <vexcl/operations.hpp>

// Include boost.preprocessor header if variadic templates are not available.
// Also include it if we use gcc v4.6.
// This is required due to bug http://gcc.gnu.org/bugzilla/show_bug.cgi?id=35722
#if defined(BOOST_NO_VARIADIC_TEMPLATES) || (defined(__GNUC__) && !defined(__clang__) && __GNUC__ == 4 && __GNUC_MINOR__ == 6)
#  include <boost/preprocessor/repetition.hpp>
#  ifndef VEXCL_MAX_ARITY
#    define VEXCL_MAX_ARITY BOOST_PROTO_MAX_ARITY
#  endif
#endif
namespace vex {

struct mba_terminal {};

typedef vector_expression<
    typename boost::proto::terminal< mba_terminal >::type
    > mba_terminal_expression;

template <class MBA, class ExprTuple>
struct mba_interp : public mba_terminal_expression {
    typedef typename MBA::value_type value_type;

    const MBA      &cloud;
    const ExprTuple coord;

    mba_interp(const MBA &cloud, const ExprTuple coord)
        : cloud(cloud), coord(coord) {}
};

namespace detail {
    // Compile time value of N^M.
    template <size_t N, size_t M>
    struct power : std::integral_constant<size_t, N * power<N, M-1>::value> {};

    template <size_t N>
    struct power<N, 0> : std::integral_constant<size_t, 1> {};

    // Nested loop counter of compile-time size (M loops of size N).
    template <size_t N, size_t M>
    class scounter {
        public:
            scounter() : idx(0) {
                std::fill(i.begin(), i.end(), static_cast<size_t>(0));
            }

            size_t operator[](size_t d) const {
                return i[d];
            }

            scounter& operator++() {
                for(size_t d = M; d--; ) {
                    if (++i[d] < N) break;
                    i[d] = 0;
                }

                ++idx;

                return *this;
            }

            operator size_t() const {
                return idx;
            }

            bool valid() const {
                return idx < power<N, M>::value;
            }
        private:
            size_t idx;
            std::array<size_t, M> i;
    };

    // Nested loop counter of run-time size (M loops of given sizes).
    template <size_t M>
    class dcounter {
        public:
            dcounter(const std::array<size_t, M> &N)
                : idx(0),
                  size(std::accumulate(N.begin(), N.end(),
                            static_cast<size_t>(1), std::multiplies<size_t>())),
                  N(N)
            {
                std::fill(i.begin(), i.end(), static_cast<size_t>(0));
            }

            size_t operator[](size_t d) const {
                return i[d];
            }

            dcounter& operator++() {
                for(size_t d = M; d--; ) {
                    if (++i[d] < N[d]) break;
                    i[d] = 0;
                }

                ++idx;

                return *this;
            }

            operator size_t() const {
                return idx;
            }

            bool valid() const {
                return idx < size;
            }
        private:
            size_t idx, size;
            std::array<size_t, M> N, i;
    };
} // namespace detail

/// Scattered data interpolation with multilevel B-Splines.
template <size_t NDIM, typename real = double>
class mba {
    public:
        typedef real value_type;
        typedef std::array<real,   NDIM> point;
        typedef std::array<size_t, NDIM> index;

        static const size_t ndim = NDIM;

        std::vector< backend::command_queue >       queue;
        std::vector< backend::device_vector<real> > phi;
        point xmin, hinv;
        index n, stride;

        /** Creates the approximation functor.
         * `cmin` and `cmax` specify the domain boundaries, `coo` and `val`
         * contain coordinates and values of the data points. `grid` is the
         * initial control grid size. The approximation hierarchy will have at
         * most `levels` and will stop when the desired approximation precision
         * `tol` will be reached.
         */
        mba(
                const std::vector<backend::command_queue> &queue,
                const point &cmin, const point &cmax,
                const std::vector<point> &coo, std::vector<real> val,
                std::array<size_t, NDIM> grid, size_t levels = 8, real tol = 1e-8
           ) : queue(queue)
        {
            init(cmin, cmax, coo.begin(), coo.end(), val.begin(), grid, levels, tol);
        }

        /** Creates the approximation functor.
         * `cmin` and `cmax` specify the domain boundaries. Coordinates and
         * values of the data points are passed as iterator ranges. `grid` is
         * the initial control grid size. The approximation hierarchy will have
         * at most `levels` and will stop when the desired approximation
         * precision `tol` will be reached.
         */
        template <class CooIter, class ValIter>
        mba(
                const std::vector<backend::command_queue> &queue,
                const point &cmin, const point &cmax,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                std::array<size_t, NDIM> grid, size_t levels = 8, real tol = 1e-8
           ) : queue(queue)
        {
            init(cmin, cmax, coo_begin, coo_end, val_begin, grid, levels, tol);
        }

#if !defined(BOOST_NO_VARIADIC_TEMPLATES) && ((!defined(__GNUC__) || (__GNUC__ > 4 || __GNUC__ == 4 && __GNUC_MINOR__ > 6)) || defined(__clang__))
        /// Provide interpolated values at given coordinates.
        template <class... Expr>
        auto operator()(const Expr&... expr) const ->
            mba_interp< mba, boost::tuple<const Expr&...> >
        {
            static_assert(sizeof...(Expr) == NDIM, "Wrong number of parameters");
            return mba_interp< mba, boost::tuple<const Expr&...> >(*this, boost::tie(expr...));
        }
#else

#define VEXCL_FUNCALL_OPERATOR(z, n, data)                                     \
  template <BOOST_PP_ENUM_PARAMS(n, class Expr)>                               \
  mba_interp<mba, boost::tuple<BOOST_PP_ENUM_BINARY_PARAMS(                    \
                      n, const Expr, &BOOST_PP_INTERCEPT)> >                   \
  operator()(BOOST_PP_ENUM_BINARY_PARAMS(n, const Expr, &expr)) {              \
    return mba_interp<mba, boost::tuple<BOOST_PP_ENUM_BINARY_PARAMS(           \
                               n, const Expr, &BOOST_PP_INTERCEPT)> >(         \
        *this, boost::tie(BOOST_PP_ENUM_PARAMS(n, expr)));                     \
  }

BOOST_PP_REPEAT_FROM_TO(1, 10, VEXCL_FUNCALL_OPERATOR, ~)

#undef VEXCL_FUNCALL_OPERATOR
#endif
    private:
        template <class CooIter, class ValIter>
        void init(
                const point &cmin, const point &cmax,
                CooIter coo_begin, CooIter coo_end, ValIter val_begin,
                std::array<size_t, NDIM> grid, size_t levels = 8, real tol = 1e-8
                )
        {
            for(size_t k = 0; k < NDIM; ++k)
                assert(grid[k] > 1);

            double res0 = std::accumulate(
                    val_begin, val_begin + (coo_end - coo_begin),
                    static_cast<real>(0),
                    [](real sum, real v) { return sum + v * v; }
                    );

            std::unique_ptr<lattice> psi(
                    new lattice(cmin, cmax, grid, coo_begin, coo_end, val_begin)
                    );
            double res = psi->update_data(coo_begin, coo_end, val_begin);
#ifdef VEXCL_MBA_VERBOSE
            std::cout << "level  0: res = " << std::scientific << res << std::endl;
#endif

            for (size_t k = 1; (res > res0 * tol) && (k < levels); ++k) {
                for(size_t d = 0; d < NDIM; ++d) grid[d] = 2 * grid[d] - 1;

                std::unique_ptr<lattice> f(
                        new lattice(cmin, cmax, grid, coo_begin, coo_end, val_begin)
                        );
                res = f->update_data(coo_begin, coo_end, val_begin);
#ifdef VEXCL_MBA_VERBOSE
                std::cout << "level " << k << std::scientific << ": res = " << res << std::endl;
#endif

                f->append_refined(*psi);
                psi = std::move(f);
            }

            xmin   = psi->xmin;
            hinv   = psi->hinv;
            n      = psi->n;
            stride = psi->stride;

            phi.reserve(queue.size());

            for(auto q = queue.begin(); q != queue.end(); ++q)
                phi.push_back( backend::device_vector<real>(
                            *q, psi->phi.size(), psi->phi.data(), backend::MEM_READ_ONLY
                            ) );
        }

        // Control lattice.
        struct lattice {
            point xmin, hinv;
            index n, stride;
            std::vector<real> phi;

            template <class CooIter, class ValIter>
            lattice(
                    const point &cmin, const point &cmax, std::array<size_t, NDIM> grid,
                    CooIter coo_begin, CooIter coo_end, ValIter val_begin
                   ) : xmin(cmin), n(grid)
            {
                for(size_t d = 0; d < NDIM; ++d) {
                    hinv[d] = (grid[d] - 1) / (cmax[d] - cmin[d]);
                    xmin[d] -= 1 / hinv[d];
                    n[d]    += 2;
                }

                stride[NDIM - 1] = 1;
                for(size_t d = NDIM - 1; d--; )
                    stride[d] = stride[d + 1] * n[d + 1];

                std::vector<real> delta(n[0] * stride[0], 0.0);
                std::vector<real> omega(n[0] * stride[0], 0.0);

                auto p = coo_begin;
                auto v = val_begin;
                for(; p != coo_end; ++p, ++v) {
                    if (!contained(cmin, cmax, *p)) continue;

                    index i;
                    point s;

                    for(size_t d = 0; d < NDIM; ++d) {
                        real u = ((*p)[d] - xmin[d]) * hinv[d];
                        i[d] = static_cast<size_t>(std::floor(u) - 1);
                        s[d] = u - std::floor(u);
                    }

                    std::array<real, detail::power<4, NDIM>::value> w;
                    real sw2 = 0;

                    for(detail::scounter<4, NDIM> d; d.valid(); ++d) {
                        real buf = 1;
                        for(size_t k = 0; k < NDIM; ++k)
                            buf *= B(d[k], s[k]);

                        w[d] = buf;
                        sw2 += buf * buf;
                    }

                    for(detail::scounter<4, NDIM> d; d.valid(); ++d) {
                        real phi = (*v) * w[d] / sw2;

                        size_t idx = 0;
                        for(size_t k = 0; k < NDIM; ++k) {
                            assert(i[k] + d[k] < n[k]);

                            idx += (i[k] + d[k]) * stride[k];
                        }

                        real w2 = w[d] * w[d];

                        assert(idx < delta.size());

                        delta[idx] += w2 * phi;
                        omega[idx] += w2;
                    }
                }

                phi.resize(omega.size());

                for(auto w = omega.begin(), d = delta.begin(), f = phi.begin();
                        w != omega.end();
                        ++w, ++d, ++f
                   )
                {
                    if (std::fabs(*w) < 1e-32)
                        *f = 0;
                    else
                        *f = (*d) / (*w);
                }
            }

            // Get interpolated value at given position.
            real operator()(const point &p) const {
                index i;
                point s;

                for(size_t d = 0; d < NDIM; ++d) {
                    real u = (p[d] - xmin[d]) * hinv[d];
                    i[d] = static_cast<size_t>(std::floor(u) - 1);
                    s[d] = u - std::floor(u);
                }

                real f = 0;

                for(detail::scounter<4, NDIM> d; d.valid(); ++d) {
                    real w = 1;
                    for(size_t k = 0; k < NDIM; ++k)
                        w *= B(d[k], s[k]);

                    f += w * get(i, d);
                }

                return f;
            }

            // Subtract interpolated values from data points.
            template <class CooIter, class ValIter>
            real update_data(
                    CooIter coo_begin, CooIter coo_end, ValIter val_begin
                    ) const
            {
                auto c = coo_begin;
                auto v = val_begin;

                real res = 0;

                for(; c != coo_end; ++c, ++v) {
                    *v -= (*this)(*c);

                    res += (*v) * (*v);
                }

                return res;
            }

            // Refine r and append it to the current control lattice.
            void append_refined(const lattice &r) {
                static const std::array<real, 5> s = {{
                    0.125, 0.500, 0.750, 0.500, 0.125
                }};

                for(detail::dcounter<NDIM> i(r.n); i.valid(); ++i) {
                    real f = r.phi[i];
                    for(detail::scounter<5, NDIM> d; d.valid(); ++d) {
                        index j;
                        bool skip = false;
                        size_t idx = 0;
                        for(size_t k = 0; k < NDIM; ++k) {
                            j[k] = 2 * i[k] + d[k] - 3;
                            if (j[k] >= n[k]) { skip = true; break; }

                            idx += j[k] * stride[k];
                        }

                        if (skip) continue;

                        real c = 1;
                        for(size_t k = 0; k < NDIM; ++k) c *= s[d[k]];

                        phi[idx] += f * c;
                    }
                }
            }

            private:
                // Value of k-th B-Spline at t.
                static inline real B(size_t k, real t) {
                    assert(0 <= t && t < 1);
                    assert(k < 4);

                    switch (k) {
                        case 0:
                            return (t * (t * (-t + 3) - 3) + 1) / 6;
                        case 1:
                            return (t * t * (3 * t - 6) + 4) / 6;
                        case 2:
                            return (t * (t * (-3 * t + 3) + 3) + 1) / 6;
                        case 3:
                        default:
                            return t * t * t / 6;
                    }
                }

                // x is within [xmin, xmax].
                static bool contained(
                        const point &xmin, const point &xmax, const point &x)
                {
                    for(size_t d = 0; d < NDIM; ++d) {
                        static const real eps = 1e-12;

                        if (x[d] - eps <  xmin[d]) return false;
                        if (x[d] + eps >= xmax[d]) return false;
                    }

                    return true;
                }

                // Get value of phi at index (i + d).
                template <class Shift>
                inline real get(const index &i, const Shift &d) const {
                    size_t idx = 0;

                    for(size_t k = 0; k < NDIM; ++k) {
                        size_t j = i[k] + d[k];

                        if (j >= n[k]) return 0;
                        idx += j * stride[k];
                    }

                    return phi[idx];
                }
        };
};

namespace traits {

template <>
struct is_vector_expr_terminal< mba_terminal > : std::true_type {};

template <>
struct proto_terminal_is_value< mba_terminal > : std::true_type {};

template <class MBA, class ExprTuple>
struct terminal_preamble< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple>&,
            const backend::command_queue&, const std::string &prm_name,
            detail::kernel_generator_state_ptr)
    {
        typedef typename MBA::value_type real;

        std::string B = prm_name + "_B";

        src.begin_function<real>(B + "0");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return (t * (t * (-t + 3) - 3) + 1) / 6;";
        src.end_function();

        src.begin_function<real>(B + "1");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return (t * t * (3 * t - 6) + 4) / 6;";
        src.end_function();

        src.begin_function<real>(B + "2");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return (t * (t * (-3 * t + 3) + 3) + 1) / 6;";
        src.end_function();

        src.begin_function<real>(B + "3");
        src.begin_function_parameters();
        src.template parameter<real>("t");
        src.end_function_parameters();
        src.new_line() << "return t * t * t / 6;";
        src.end_function();

        src.begin_function<real>(prm_name + "_mba");
        src.begin_function_parameters();

        for(size_t k = 0; k < MBA::ndim; ++k)
            src.template parameter<real>("x") << k;

        for(size_t k = 0; k < MBA::ndim; ++k) {
            src.template parameter<real>("c" + std::to_string(k));
            src.template parameter<real>("h" + std::to_string(k));
            src.parameter<size_t>("n" + std::to_string(k));
            src.parameter<size_t>("m" + std::to_string(k));
        }

        src.template parameter< global_ptr<const real> >("phi");
        src.end_function_parameters();
        src.new_line() << type_name<real>() << " u;";
        for(size_t k = 0; k < MBA::ndim; ++k) {
            src.new_line() << "u = (x" << k << " - c" << k << ") * h" << k << ";";
            src.new_line() << type_name<size_t>() << " i" << k << " = floor(u) - 1;";
            src.new_line() << type_name<real>() << " s" << k << " = u - floor(u);";
        }
        src.new_line() << type_name<real>() << " f = 0;";
        src.new_line() << type_name<size_t>() << " j, idx;";

        for(detail::scounter<4,MBA::ndim> d; d.valid(); ++d) {
            src.new_line() << "idx = 0;";
            for(size_t k = 0; k < MBA::ndim; ++k) {
                src.new_line() << "j = i" << k << " + " << d[k] << ";";
                src.new_line() << "if (j < n" << k << ")";
                src.open("{").new_line() << "idx += j * m" << k << ";";
            }

            src.new_line() << "f += ";
            for(size_t k = 0; k < MBA::ndim; ++k) {
                if (k) src << " * ";
                src << B << d[k] << "(s" << k << ")";
            }

            src << " * phi[idx];";

            for(size_t k = 0; k < MBA::ndim; ++k)
                src.close("}");
        }
        src.new_line() << "return f;";
        src.end_function();
    }
};

template <class MBA, class ExprTuple>
struct kernel_param_declaration< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple> &term,
            const backend::command_queue &queue, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
    {
        typedef typename MBA::value_type real;
        boost::fusion::for_each(term.coord, prmdecl(src, queue, prm_name, state));

        for(size_t k = 0; k < MBA::ndim; ++k) {
            src.template parameter<real>(prm_name + "_c" + std::to_string(k));
            src.template parameter<real>(prm_name + "_h" + std::to_string(k));
            src.parameter<size_t>(prm_name + "_n" + std::to_string(k));
            src.parameter<size_t>(prm_name + "_m" + std::to_string(k));
        }

        src.parameter< global_ptr<const real> >(prm_name + "_phi");
    }

    struct prmdecl {
        backend::source_generator &s;
        const backend::command_queue &queue;
        const std::string &prm_name;
        detail::kernel_generator_state_ptr state;
        mutable int pos;

        prmdecl(backend::source_generator &s,
                const backend::command_queue &queue, const std::string &prm_name,
                detail::kernel_generator_state_ptr state
            ) : s(s), queue(queue), prm_name(prm_name), state(state), pos(0)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            std::ostringstream prefix;
            prefix << prm_name << "_x" << pos;
            detail::declare_expression_parameter ctx(s, queue, prefix.str(), state);
            detail::extract_terminals()(boost::proto::as_child(expr), ctx);

            pos++;
        }
    };
};

template <class MBA, class ExprTuple>
struct local_terminal_init< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple> &term,
            const backend::command_queue &queue, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
    {
        boost::fusion::for_each(term.coord, local_init(src, queue, prm_name, state));
    }

    struct local_init {
        backend::source_generator &s;
        const backend::command_queue &queue;
        const std::string &prm_name;
        detail::kernel_generator_state_ptr state;
        mutable int pos;

        local_init(backend::source_generator &s,
                const backend::command_queue &queue, const std::string &prm_name,
                detail::kernel_generator_state_ptr state
            ) : s(s), queue(queue), prm_name(prm_name), state(state), pos(0)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            std::ostringstream prefix;
            prefix << prm_name << "_x" << pos;

            detail::output_local_preamble init_ctx(s, queue, prefix.str(), state);
            boost::proto::eval(boost::proto::as_child(expr), init_ctx);

            pos++;
        }
    };
};

template <class MBA, class ExprTuple>
struct partial_vector_expr< mba_interp<MBA, ExprTuple> > {
    static void get(backend::source_generator &src,
            const mba_interp<MBA, ExprTuple> &term,
            const backend::command_queue &queue, const std::string &prm_name,
            detail::kernel_generator_state_ptr state)
    {
        src << prm_name << "_mba(";

        boost::fusion::for_each(term.coord, buildexpr(src, queue, prm_name, state));

        for(size_t k = 0; k < MBA::ndim; ++k) {
            src << ", " << prm_name << "_c" << k
                << ", " << prm_name << "_h" << k
                << ", " << prm_name << "_n" << k
                << ", " << prm_name << "_m" << k;
        }

        src << ", " << prm_name << "_phi)";
    }

    struct buildexpr {
        backend::source_generator &s;
        const backend::command_queue &queue;
        const std::string &prm_name;
        detail::kernel_generator_state_ptr state;
        mutable int pos;

        buildexpr(backend::source_generator &s,
                const backend::command_queue &queue, const std::string &prm_name,
                detail::kernel_generator_state_ptr state
            ) : s(s), queue(queue), prm_name(prm_name), state(state), pos(0)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            if(pos) s << ", ";

            std::ostringstream prefix;
            prefix << prm_name << "_x" << pos;

            detail::vector_expr_context ctx(s, queue, prefix.str(), state);
            boost::proto::eval(boost::proto::as_child(expr), ctx);

            pos++;
        }
    };
};

template <class MBA, class ExprTuple>
struct kernel_arg_setter< mba_interp<MBA, ExprTuple> > {
    static void set(const mba_interp<MBA, ExprTuple> &term,
            backend::kernel &kernel, unsigned part, size_t index_offset,
            detail::kernel_generator_state_ptr state)
    {

        boost::fusion::for_each(term.coord,
                setargs(kernel, part, index_offset, state));

        for(size_t k = 0; k < MBA::ndim; ++k) {
            kernel.push_arg(term.cloud.xmin[k]);
            kernel.push_arg(term.cloud.hinv[k]);
            kernel.push_arg(term.cloud.n[k]);
            kernel.push_arg(term.cloud.stride[k]);
        }
        kernel.push_arg(term.cloud.phi[part]);
    }

    struct setargs {
        backend::kernel &kernel;
        unsigned part;
        size_t index_offset;
        detail::kernel_generator_state_ptr state;

        setargs(
                backend::kernel &kernel, unsigned part, size_t index_offset,
                detail::kernel_generator_state_ptr state
               )
            : kernel(kernel), part(part), index_offset(index_offset), state(state)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            detail::set_expression_argument ctx(kernel, part, index_offset, state);
            detail::extract_terminals()( boost::proto::as_child(expr), ctx);
        }
    };
};

template <class MBA, class ExprTuple>
struct expression_properties< mba_interp<MBA, ExprTuple> > {
    static void get(const mba_interp<MBA, ExprTuple> &term,
            std::vector<backend::command_queue> &queue_list,
            std::vector<size_t> &partition,
            size_t &size
            )
    {
        boost::fusion::for_each(term.coord, extrprop(queue_list, partition, size));
    }

    struct extrprop {
        std::vector<backend::command_queue> &queue_list;
        std::vector<size_t> &partition;
        size_t &size;

        extrprop(std::vector<backend::command_queue> &queue_list,
            std::vector<size_t> &partition, size_t &size
            ) : queue_list(queue_list), partition(partition), size(size)
        {}

        template <class Expr>
        void operator()(const Expr &expr) const {
            if (queue_list.empty()) {
                detail::get_expression_properties prop;
                detail::extract_terminals()(boost::proto::as_child(expr), prop);

                queue_list = prop.queue;
                partition  = prop.part;
                size       = prop.size;
            }
        }
    };
};

} //namespace traits

} // namespace vex

#endif