//
// Created by Vasiliy Ershov on 08/11/2016.
//

#ifndef PROJECT_NORMAL_QUALITY_MODEL_HPP
#define PROJECT_NORMAL_QUALITY_MODEL_HPP

#include <common/utils/parallel/openmp_wrapper.h>
#include <array>
#include <boost/math/special_functions/binomial.hpp>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/trigamma.hpp>
#include <vector>
#include "config_struct.hpp"
#include "kmer_data.hpp"
#include "quality_thresholds_estimator.h"
#include "thread_utils.h"
#include "valid_hkmer_generator.hpp"
//

namespace n_normal_model {

struct QualityTransform {
  double bias_;

  QualityTransform(double bias = 60.0) : bias_(bias) {}

  double Apply(double quality, double count) const {
    return quality / (count + 60);
  }
};

class NormalDistribution {
 private:
  double mean_;
  double sigma_sqr_;

 public:
  NormalDistribution(const NormalDistribution&) = default;

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

  NormalDistribution(const double mean = 0, const double sigma = 1)
      : mean_(mean), sigma_sqr_(sigma) {}

  inline double GetMean() const { return mean_; }

  inline double GetSigmaSqr() const { return sigma_sqr_; }

  double LogLikelihood(double x) const {
    return -0.5 *
           ((x - mean_) * (x - mean_) / sigma_sqr_ + log(2 * M_PI * sigma_sqr_));
  }

  double LogLikelihoodFromStats(const double sum,
                                const double sum2,
                                const double weight) const {
    return -0.5 * ((sum2 - 2 * sum * mean_ + weight * mean_ * mean_) / sigma_sqr_ +
                   weight * log(2 * M_PI * sigma_sqr_));
  }

  static NormalDistribution FromStats(const double sum,
                                      const double sum2,
                                      const double weight) {
    const double mu = sum / weight;
    const double var = sum2 / weight - mu * mu;
    return NormalDistribution(mu, var);
  }
};

class NormalMixture {
 private:
  NormalDistribution first_;
  NormalDistribution second_;
  double first_weight_;

 public:
  NormalMixture() : first_weight_(0) {}

  NormalMixture(const NormalDistribution& first,
                const NormalDistribution& second,
                double weight)
      : first_(first), second_(second), first_weight_(weight) {}

  const NormalDistribution& GetFirst() const { return first_; }

  const NormalDistribution& GetSecond() const { return second_; }

  double GetFirstWeight() const { return first_weight_; }

  double LogLikelihood(double x) const {
    return log(first_weight_ * exp(first_.LogLikelihood(x)) +
               (1 - first_weight_) * exp(second_.LogLikelihood(x)));
  }

  double FirstComponentPosterior(double x) const {
    double firstLL = first_.LogLikelihood(x) + log(first_weight_);
    double secondLL = second_.LogLikelihood(x) + log(1.0 - first_weight_);
    const double expDiff = exp(secondLL - firstLL);

    return std::isfinite(expDiff) ? -log(1.0 + exp(secondLL - firstLL))
                                  : firstLL - secondLL;
  }
};

class Binarizer {
 private:
  std::vector<double> borders_;

 public:
  Binarizer() {
    for (int i = 17; i < 30; ++i) {
      borders_.push_back(i);
    }
  }

  Binarizer(const std::vector<double> &borders) : borders_(borders) {}

  int GetBin(double value) const {
    uint index = 0;
    while (index < borders_.size() && value > borders_[index]) {
      ++index;
    }
    return index;
  }

  size_t GetBinCount() const { return borders_.size() + 1; }

  double GetBorder(int bin) {
    --bin;
    bin = std::min(bin, (int)(borders_.size() - 1));
    if (bin < 0) {
      return 0;
    }
    return borders_[bin];
  }
};

class NormalClusterModel {
 private:
  std::vector<NormalMixture> mixtures_;
  Binarizer binarizer_;
  std::vector<double> median_qualities_;
  QualityTransform trans_;
  double lower_quality_threshold_;

  static std::vector<double> left_likelihoods_;
  static std::vector<double> equal_likelihoods_;
  static std::vector<double> right_likelihoods_;

 public:
  NormalClusterModel() {}

  NormalClusterModel(const std::vector<NormalMixture>& mixtures,
                      const Binarizer& binarizer,
                      const std::vector<double>& medianQualities,
                      const QualityTransform& trans)
      : mixtures_(mixtures),
        binarizer_(binarizer),
        median_qualities_(medianQualities),
        trans_(trans) {
    lower_quality_threshold_ = cfg::get().noise_filter_count_threshold;  // threshold >= 10 ? 1 : 0;
  }

  NormalClusterModel(const NormalClusterModel& other) = default;

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

  bool NeedSubcluster(const hammer::KMerStat& stat) const {
    return stat.count > 15 && GenomicLogLikelihood(stat) > -0.0001;
  }

  double StatTransform(const hammer::KMerStat& stat) const {
    return trans_.Apply(stat.qual, stat.count);
  }

  double GenomicLogLikelihood(const hammer::KMerStat& stat) const {
    return GenomicLogLikelihood(binarizer_.GetBin((double)GetKmerBinIdx(stat.kmer)),
                                stat.qual, stat.count);
  }

  bool IsHighQuality(const hammer::KMerStat& stat) const {
    const auto bin = binarizer_.GetBin((double)GetKmerBinIdx(stat.kmer));
    return trans_.Apply(stat.qual, stat.count) <= median_qualities_[bin];
  }

  double GenomicLogLikelihood(int bin, double quality, double count) const {
    if (count <= lower_quality_threshold_) {
      return -1e5;
    }
    const double x = trans_.Apply(quality, count);
    return mixtures_[bin].FirstComponentPosterior(x);
  }

  static size_t GetKmerBinIdx(const hammer::HKMer& kmer) {
    if (kmer.size() > 21) {
      return 1 + kmer.max_run_length();
    } else {
      return 0;
    }
  }

  static double ErrorLogLikelihood(int from, int to) {
    int diff = std::abs(from - to);
    from = std::max(from, 0);
    --from;
    int sign = from > to ? -1 : 1;
    from = std::min((int)equal_likelihoods_.size() - 1, from);
    if (diff == 0) {
      return equal_likelihoods_[from];
    }
    if (sign == -1) {
      return left_likelihoods_[from] * diff;
    }
    return right_likelihoods_[from] * diff;
  }
};

class NormalMixtureEstimator {
 private:
  uint num_threads_;
  size_t max_iterations_;
  bool calc_likelihoods_;

 private:
  std::vector<double> BuildPriors(const std::vector<double>& observations) const {
    double threshold = SimpleTwoClassClustering::SimpleThresholdEstimation(
                           observations.begin(), observations.end())
                           .split_;

    std::vector<double> priors(observations.size());

#pragma omp parallel for num_threads(num_threads_)
    for (size_t i = 0; i < observations.size(); ++i) {
      priors[i] = observations[i] <= threshold ? 1 : 0;
    }

    return priors;
  }

  struct Stats {
    double sum_left_ = 0;
    double sum2_left_ = 0;
    double weight_left_ = 0;
    double sum_right_ = 0;
    double sum2_right_ = 0;

    Stats& operator+=(const Stats& other) {
      if (this != &other) {
        sum_left_ += other.sum_left_;
        sum2_left_ += other.sum2_left_;
        sum_right_ += other.sum_right_;
        sum2_right_ += other.sum2_right_;
        weight_left_ += other.weight_left_;
      }
      return *this;
    }
  };

 public:
  NormalMixtureEstimator(uint num_threads,
                         size_t max_iterations,
                          bool calc_likelihood)
      : num_threads_(num_threads),
        max_iterations_(max_iterations),
        calc_likelihoods_(calc_likelihood) {}

  NormalMixture Estimate(std::vector<double>& observations) const {
    std::sort(observations.begin(), observations.end(), std::greater<double>());
    observations.resize(observations.size());
    std::reverse(observations.begin(), observations.end());

    std::vector<double> priors = BuildPriors(observations);

    NormalMixture mixture;

    for (size_t iter = 0; iter < max_iterations_; ++iter) {
      auto stats =
          n_computation_utils::ParallelStatisticsCalcer<Stats>(num_threads_)
              .Calculate(observations.size(),
                         [&]() -> Stats { return Stats(); },
                         [&](Stats& stat, size_t k) {
                           const double x = observations[k];
                           const double w = priors[k];
                           stat.sum2_left_ += w * x * x;
                           stat.sum_left_ += w * x;
                           stat.weight_left_ += w;
                           stat.sum2_right_ += (1 - w) * x * x;
                           stat.sum_right_ += (1 - w) * x;
                         });

      mixture =
          NormalMixture(NormalDistribution::FromStats(
                             stats.sum_left_, stats.sum2_left_, stats.weight_left_),
                         NormalDistribution::FromStats(
                             stats.sum_right_, stats.sum2_right_,
                             (double)observations.size() - stats.weight_left_),
                         stats.weight_left_ / (double)observations.size());

// expectation
#pragma omp parallel for num_threads(num_threads_)
      for (size_t i = 0; i < observations.size(); ++i) {
        priors[i] = exp(mixture.FirstComponentPosterior(observations[i]));
      }

      if (calc_likelihoods_) {
        double ll = 0;
        for (size_t i = 0; i < observations.size(); ++i) {
          const double x = observations[i];
          ll += mixture.LogLikelihood(x);
        }
        INFO("LogLikelihood: " << ll);
      }

      if (iter == 0 || iter == (max_iterations_ - 1)) {
        const double llFirst = mixture.GetFirst().LogLikelihoodFromStats(
            stats.sum_left_, stats.sum2_left_, stats.weight_left_);
        INFO("Likelihood first: " << llFirst);
        const double llSecond = mixture.GetSecond().LogLikelihoodFromStats(
            stats.sum_right_, stats.sum2_right_,
            (double)observations.size() - stats.weight_left_);
        INFO("Likelihood second: " << llSecond);
        INFO("First weights: " << mixture.GetFirstWeight());
      }
    }
    return mixture;
  };
};

// this class estimate prior distribution.
class ModelEstimator {
 private:
  const KMerData& data_;
  uint num_threads_;
  size_t max_iterations_;
  bool is_calc_likelihood_;

 public:
  ModelEstimator(const KMerData& data,
                 uint num_threads = 16,
                 size_t maxIterations = 40,
                 bool calc_likelihood = false)
      : data_(data),
        num_threads_(num_threads),
        max_iterations_(maxIterations),
        is_calc_likelihood_(calc_likelihood) {}

  NormalClusterModel Estimate(
      const std::vector<std::vector<size_t> >& clusters) {
    QualityTransform trans;

    std::vector<size_t> cluster_center;
    {
      cluster_center.resize(clusters.size());
#pragma omp parallel for num_threads(num_threads_)
      for (size_t i = 0; i < clusters.size(); ++i) {
        auto& cluster = clusters[i];

        double best_qual =
            trans.Apply(data_[cluster[0]].qual, data_[cluster[0]].count);
        size_t bestIdx = cluster[0];

        for (auto idx : cluster) {
          const auto qual = trans.Apply(data_[idx].qual, data_[idx].count);
          if (qual < best_qual ||
              (qual == best_qual &&
               data_[idx].kmer.size() < data_[bestIdx].kmer.size())) {
            best_qual = qual;
            bestIdx = idx;
          }
          cluster_center[i] = bestIdx;
        }
      }
    }

    std::vector<std::vector<double> > qualities;
    qualities.reserve(16);
    const size_t sampleMaxThreshold = (size_t)1e9;
    const size_t min_sample_size = (size_t)1e4;

    {
      double skip_threshold = cfg::get().noise_filter_count_threshold;  // threshold >= 10 ? 1 : 0;

      for (size_t i = 0; i < cluster_center.size(); ++i) {
        const auto& stat = data_[cluster_center[i]];

        if (stat.count <= skip_threshold) {
          continue;
        }
        const size_t bin = NormalClusterModel::GetKmerBinIdx(stat.kmer);

        if (bin >= qualities.size()) {
          qualities.resize(bin + 1);
        }

        if (qualities[bin].size() > sampleMaxThreshold) {
          continue;
        }
        auto trans_qual = trans.Apply(stat.qual, stat.count);
        qualities[bin].push_back(trans_qual);
      }
    }

    std::vector<NormalMixture> models;
    std::vector<double> borders;
    std::vector<double> median_qualities;

    size_t total_count = 0;
    for (const auto& qual : qualities) {
      total_count += qual.size();
    }
    assert(qualities[1].size() == 0);

    {
      auto model = NormalMixtureEstimator(num_threads_, max_iterations_, is_calc_likelihood_).Estimate(qualities[0]);

      const double median_quality = FindHighQualityThreshold(qualities[0], model);
      INFO("For kmer length <= 21");
      INFO("Median quality " << median_quality);
      INFO("Sample size " << qualities[0].size());
      INFO("Genomic dist: " << model.GetFirst().GetMean() << " "
                            << model.GetFirst().GetSigmaSqr());
      INFO("NonGenomic dist: " << model.GetSecond().GetMean() << " "
                               << model.GetSecond().GetSigmaSqr());
      models.push_back(model);
      median_qualities.push_back(median_quality);
      borders.push_back(0);
      total_count -= qualities[0].size();
    }

    const auto len_limit = std::min(qualities.size(), 7UL);
    for (uint max_run_len = 2; max_run_len < len_limit; ++max_run_len) {
      if (total_count < min_sample_size) {
        break;
      }

      const size_t bin = max_run_len + 1;
      auto bin_qualities = qualities[bin];
      total_count -= bin_qualities.size();

      if (bin_qualities.size() < min_sample_size) {
        if (bin + 1 < qualities.size()) {
          qualities[bin + 1].insert(qualities[bin + 1].end(),
                                    bin_qualities.begin(),
                                    bin_qualities.end());
        }
        continue;
      }

      auto model = NormalMixtureEstimator(num_threads_, max_iterations_, is_calc_likelihood_).Estimate(bin_qualities);

      const double median_quality = FindHighQualityThreshold(bin_qualities, model);

      INFO("Sample size " << bin_qualities.size());
      INFO("Median quality " << median_quality);
      INFO("For max run length >= " << max_run_len);
      INFO("Genomic dist: " << model.GetFirst().GetMean() << " "
                            << model.GetFirst().GetSigmaSqr());
      INFO("NonGenomic dist: " << model.GetSecond().GetMean() << " "
                               << model.GetSecond().GetSigmaSqr());
      median_qualities.push_back(median_quality);
      models.push_back(model);
      borders.push_back((double)bin);
    }
    borders.resize(borders.size() - 1);

    return NormalClusterModel(models, Binarizer(borders), median_qualities,
                               trans);
  }

  double FindHighQualityThreshold(const std::vector<double>& bin_quality,
                                  const NormalMixture& model) const {
    std::vector<double> good_samples;
    good_samples.reserve(bin_quality.size());
    for (size_t i = 0; i < bin_quality.size(); ++i) {
      if (model.FirstComponentPosterior(bin_quality[i]) > -0.69) {
        good_samples.push_back(bin_quality[i]);
      }
    }

    const size_t quantile = (size_t)((double)good_samples.size() * cfg::get().dist_one_subcluster_alpha);
    std::nth_element(good_samples.begin(), good_samples.begin() + quantile,
                     good_samples.end());
    return good_samples[quantile];
  }
};

}  // namespace NNormalModel

#endif  // PROJECT_NORMAL_QUALITY_MODEL_HPP


#include <ctime>

#include <boost/random.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include "NonlinearRBFFactorType.h"

namespace Grante {

NonlinearRBFFactorType::NonlinearRBFFactorType(const std::string& name,
	const std::vector<unsigned int>& card,
	unsigned int data_size, unsigned int rbf_basis_count, double log_beta)
	: FactorType(name, card, data_size), rbfnet(rbf_basis_count, data_size),
		rbf_basis_count(rbf_basis_count) {
	InitializeProdCard();
	assert(rbf_basis_count > 0);

	assert((boost::math::isnan)(log_beta) == false);
	rbfnet.FixBeta(log_beta);
	size_t wdim = prod_card * rbfnet.ParameterDimension();

	// Initialize weight vector randomly
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > randu(rgen, rdestu);

	// FIXME: better initialization concepts
	w.resize(wdim);
	std::fill(w.begin(), w.end(), 0.0);
	size_t wbase = 0;
	for (unsigned int ri = 0; ri < prod_card; ++ri) {
		// Initialize alpha_n
		for (unsigned int wi = 0; wi < rbf_basis_count; ++wi)
			w[wbase + wi] = randu() - 0.5;

		// Initialize c_n
		for (unsigned int wi = 0; wi < (data_size*rbf_basis_count); ++wi)
			w[wbase + rbf_basis_count + wi] = randu() - 0.5;

		wbase += rbfnet.ParameterDimension();
	}
}

NonlinearRBFFactorType::~NonlinearRBFFactorType() {
}

void NonlinearRBFFactorType::InitializeUsingTrainingData(const std::vector<
	ParameterEstimationMethod::labeled_instance_type>& training_data) {
	boost::mt19937 rgen(static_cast<const boost::uint32_t>(std::time(0))+1);
	boost::uniform_real<double> rdestu;	// range [0,1]
	boost::variate_generator<boost::mt19937,
		boost::uniform_real<double> > randu(rgen, rdestu);

	size_t wbase = 0;
	for (unsigned int ri = 0; ri < prod_card; ++ri) {
		// 1. Collect all factors that are labeled with the corresponding
		// ground truth label
		std::vector<Factor*> m_factors;
		for (unsigned int n = 0; n < training_data.size(); ++n) {
			const FactorGraph* fg = training_data[n].first;
			const FactorGraphObservation* obs = training_data[n].second;
			assert(obs->Type() == FactorGraphObservation::DiscreteLabelingType);
			const std::vector<Factor*>& factors = fg->Factors();
			for (unsigned int fi = 0; fi < factors.size(); ++fi) {
				if (factors[fi]->Type()->Name() != Name())
					continue;

				unsigned int ei_obs =
					factors[fi]->ComputeAbsoluteIndex(obs->State());
				if (ei_obs != ri)
					continue;

				m_factors.push_back(factors[fi]);
			}
		}
		// Need to be sure there is at least one observation
		assert(m_factors.size() >= 1);
		std::cout << m_factors.size() << " samples for statepair "
			<< ri << std::endl;

		// Initialize alpha_n
		for (unsigned int wi = 0; wi < rbf_basis_count; ++wi)
			w[wbase + wi] = 1.0;

		// Initialize c_n as sample from the training set
		size_t wbi_base = 0;
		for (unsigned int bi = 0; bi < rbf_basis_count; ++bi) {
			unsigned int mi = static_cast<unsigned int>(
				randu() * static_cast<double>(m_factors.size()));
			assert(mi < m_factors.size());
			const std::vector<double>& H = m_factors[mi]->Data();

			// Copy selected training instance, perturbed
			assert(H.size() == data_size);
			for (unsigned int wi = 0; wi < data_size; ++wi) {
				w[wbase + rbf_basis_count + wbi_base + wi] =
					H[wi] + randu()*1.0e-8;
			}
			wbi_base += data_size;
		}
		wbase += rbfnet.ParameterDimension();
	}
	assert(wbase == w.size());
}

void NonlinearRBFFactorType::InitializeWeights(
	const std::vector<double>& weights) {
	assert(weights.size() == (prod_card * rbfnet.ParameterDimension()));
	this->w = weights;
}

bool NonlinearRBFFactorType::IsDataDependent() const {
	return (true);
}

void NonlinearRBFFactorType::ForwardMap(const Factor* factor,
	std::vector<double>& energies) const {
	const std::vector<double>& H = factor->Data();
	assert(H.size() == data_size);
	assert(energies.size() == prod_card);
	size_t wbase = 0;
	for (size_t ei = 0; ei < prod_card; ++ei) {
		energies[ei] = rbfnet.Evaluate(H, w, wbase);
		wbase += rbfnet.ParameterDimension();
	}
}

void NonlinearRBFFactorType::BackwardMap(const Factor* factor,
	const std::vector<double>& marginals,
	std::vector<double>& parameter_gradient, double mult) const {
	const std::vector<double>& H = factor->Data();
	assert(H.size() == data_size);
	size_t wbase = 0;
	for (size_t ei = 0; ei < prod_card; ++ei) {
		rbfnet.EvaluateGradient(H, w, parameter_gradient, wbase,
			mult * marginals[ei]);
		wbase += rbfnet.ParameterDimension();
	}
}

const RBFNetwork& NonlinearRBFFactorType::Net() const {
	return (rbfnet);
}

}


/*M///////////////////////////////////////////////////////////////////////////////////////
// IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
//
//  By downloading, copying, installing or using the software you agree to this license.
//  If you do not agree to this license, do not download, install,
//  copy or use the software.
//
//
//                           License Agreement
//                For Open Source Computer Vision Library
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2008-2011, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// 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.
//
//   * The name of the copyright holders may not be used to endorse or promote products
//     derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation 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.
//
// C by Benjamin Wassermann
//M*/

#ifndef _GEOMETRY_BASE_HPP_
#define _GEOMETRY_BASE_HPP_
#ifdef __cplusplus

//#define EIGEN_MATRIXBASE_PLUGIN "MatrixBaseAddons.h"
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include "../utility/limit.hpp"
#include <cstdlib>

namespace lsfm {
    namespace detail {
        using std::round;
        using std::abs;
        using std::log;
        using std::exp;
        using std::sqrt;
        using std::cos;
        using std::acos;
        using std::sin;
        using std::asin;
        using std::tan;
        using std::atan;
        using std::sinh;
        using std::cosh;
        using std::tanh;
        using std::pow;
        using std::atan2;
        using std::hypot;
    }

    template<class FT>
    FT getScalar(FT val){
        return val;
    }

    // note -> use Eigen::DontAlign for eigen matrix to prevent align problems, eg. transpose etc.
    // eigen seems to have problems wiht stl vector -> Eigen::aligned_allocator has to be used or
    // EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION or best swith off alignment?!
    template<int rows, int _cols>
    struct Major {
        enum { type = ((rows > 1 || rows == Eigen::Dynamic) * (_cols > 1 || _cols == Eigen::Dynamic) * Eigen::RowMajor) | Eigen::DontAlign};
		//static constexpr int type = ((rows > 1 || rows == Eigen::Dynamic) * (_cols > 1 || _cols == Eigen::Dynamic) * Eigen::RowMajor) | Eigen::DontAlign ;
    };

    // matrix base, up to 4x4
    template<class FT, int _rows, int _cols>
    class Matx : public Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type > {
    public:
        typedef FT float_type;

        typedef Eigen::Matrix<FT,_rows,_cols, Major<_rows, _cols>::type> MatrixBase;
        typedef typename MatrixBase::Base Base;
        typedef typename MatrixBase::Index Index;

        //typedef typename Base::PlainObject PlainObject;
        //using Base::base;
        //using Base::coeffRef;

        //EIGEN_DENSE_PUBLIC_INTERFACE(MyMatrixBase)

        //Matx(const MyMatrixBase& other) : Eigen::Matrix<FT,_rows,_cols>(other) {}

        template<typename OtherDerived>
        Matx(const Eigen::MatrixBase<OtherDerived>& other) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(other) {}

        template<typename OtherDerived>
        Matx(const Eigen::ReturnByValue<OtherDerived>& other) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(other) {}

        template<typename OtherDerived>
        Matx(const Eigen::EigenBase<OtherDerived> &other) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(other) {}

        Matx() : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() {}
        Matx(const FT &v0, const FT &v1): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(v0,v1) {}
        Matx(const FT &v0, const FT &v1, const FT &v2): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(v0,v1,v2) {}
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() {
            if (_rows == 4 || _cols == 4) {
                this->m_storage.data()[0] = v0;
                this->m_storage.data()[1] = v1;
                this->m_storage.data()[2] = v2;
                this->m_storage.data()[3] = v3;
                return;
            }
            *this << v0,v1,v2,v3;
        }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5, const FT &v6): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5, v6; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5, const FT &v6, const FT &v7): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5,v6,v7; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3, const FT &v4, const FT &v5, const FT &v6, const FT &v7, const FT &v8): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() {
            *this << v0,v1,v2,v3,v4,v5,v6,v7,v8;
        }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3,
             const FT &v4, const FT &v5, const FT &v6, const FT &v7,
             const FT &v8, const FT &v9, const FT &v10, const FT &v11): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5,v6,v7,v8,v9,v10,v11; }
        Matx(const FT &v0, const FT &v1, const FT &v2, const FT &v3,
             const FT &v4, const FT &v5, const FT &v6, const FT &v7,
             const FT &v8, const FT &v9, const FT &v10, const FT &v11,
             const FT &v12, const FT &v13, const FT &v14, const FT &v15): Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>() { *this << v0,v1,v2,v3,v4,v5,v6,v7,v8,v9,v10,v11,v12,v13,v14,v15; }

        explicit Matx(const FT* vals) : Eigen::Matrix<FT,_rows,_cols, Major<_rows,_cols>::type>(vals) {}

        /*Matx& operator=(const MyMatrixBase& other) {
            this->Base::_set(other);
            return *this;
        }*/

        template<typename OtherDerived>
        Matx& operator=(const Eigen::MatrixBase<OtherDerived>& other) {
            this->Base::operator=(other);
            return *this;
        }

        template<typename OtherDerived>
        Matx& operator=(const Eigen::EigenBase<OtherDerived> &other) {
            this->Base::operator=(other);
            return *this;
        }

        template<typename OtherDerived>
        Matx& operator=(const Eigen::ReturnByValue<OtherDerived>& func) {
            this->Base::operator=(func);
            return *this;
        }

        // linear access
        inline FT& operator[](Index idx) { return this->m_storage.data()[idx]; }
        inline const FT& operator[](Index idx) const { return this->m_storage.data()[idx]; }

        // point access
        inline FT& x() { return this->m_storage.data()[0]; }
        inline const FT& x() const { return this->m_storage.data()[0]; }

        inline FT& y() { return this->m_storage.data()[1]; }
        inline const FT& y() const { return this->m_storage.data()[1]; }

        inline FT& z() { return this->m_storage.data()[2]; }
        inline const FT& z() const { return this->m_storage.data()[2]; }

        inline FT& w() { return this->m_storage.data()[3]; }
        inline const FT& w() const { return this->m_storage.data()[3]; }
    };

    template<class FT>
    using Matx22 = Matx<FT,2,2>;

    template<class FT>
    using Matx23 = Matx<FT,2,3>;

    template<class FT>
    using Matx32 = Matx<FT,3,2>;

    template<class FT>
    using Matx33 = Matx<FT,3,3>;

    template<class FT>
    using Matx34 = Matx<FT,3,4>;

    template<class FT>
    using Matx43 = Matx<FT,4,3>;

    template<class FT>
    using Matx44 = Matx<FT,4,4>;

    typedef Matx22<float> Matx22f;
    typedef Matx22<double> Matx22d;

    typedef Matx23<float> Matx23f;
    typedef Matx23<double> Matx23d;

    typedef Matx32<float> Matx32f;
    typedef Matx32<double> Matx32d;

    typedef Matx33<float> Matx33f;
    typedef Matx33<double> Matx33d;

    typedef Matx34<float> Matx34f;
    typedef Matx34<double> Matx34d;

    typedef Matx43<float> Matx43f;
    typedef Matx43<double> Matx43d;

    typedef Matx44<float> Matx44f;
    typedef Matx44<double> Matx44d;

    template<class FT, int rows>
    using Vec = Matx<FT,rows,1>;

    template<class FT>
    using Vec2 = Vec<FT,2>;

    template<class FT>
    using Vec3 = Vec<FT,3>;

    template<class FT>
    using Vec4 = Vec<FT,4>;

    typedef Vec2<float> Vec2f;
    typedef Vec2<double> Vec2d;
    typedef Vec2<int> Vec2i;
    typedef Vec3<float> Vec3f;
    typedef Vec3<double> Vec3d;
    typedef Vec3<int> Vec3i;
    typedef Vec4<float> Vec4f;
    typedef Vec4<double> Vec4d;
    typedef Vec4<int> Vec4i;

    template<class FT, int cols>
    using RowVec = Matx<FT,1,cols>;

    template<class FT>
    using RowVec2 = RowVec<FT,2>;

    template<class FT>
    using RowVec3 = RowVec<FT,3>;

    template<class FT>
    using RowVec4 = RowVec<FT,4>;

    typedef RowVec2<float> RowVec2f;
    typedef RowVec2<double> RowVec2d;
    typedef RowVec3<float> RowVec3f;
    typedef RowVec3<double> RowVec3d;
    typedef RowVec4<float> RowVec4f;
    typedef RowVec4<double> RowVec4d;

    //! convert rot vec to rot matrix
    template<class FT>
    inline Matx33<FT> rodrigues(const Vec3<FT> &r) {
        Vec3<FT> axis;
        FT angle = rodrigues(r,axis);
        return Eigen::AngleAxis<FT>(angle,axis).matrix();

    }

    //! convert rot matrix to rot vec
    template<class FT>
    inline Vec3<FT> rodrigues(const Matx33<FT> &r) {
        Eigen::AngleAxis<FT> axis(r);
        return axis.axis() * axis.angle();
    }

    //! convert rot point to rot axis and angle
    template<class FT>
    inline FT rodrigues(const Vec3<FT> r, Vec3<FT> &axis) {
        FT n = r.norm();
        if (n < LIMITS<FT>::tau()) {
            axis = Vec3<FT>(FT(1),FT(0),FT(0));
            return FT(0);
        }
        axis = r / n;
        return n;
    }

    //! compose homogeneouse matrix from trans vector and rot vector
    template<class FT>
    Matx44<FT> composeHom(const Vec3<FT> &trans, const Vec3<FT> &rot = Vec3<FT>(FT(0),FT(0),FT(0))) {
        return composeHom(trans,rodrigues(rot));
    }

    //! compose homogeneouse matrix from trans vector and rot matrix
    template<class FT>
    Matx44<FT> composeHom(const Vec3<FT> &trans, const Matx33<FT> &rot) {
        return Matx44<FT>(rot(0,0),rot(0,1),rot(0,2),trans.x(),
                          rot(1,0),rot(1,1),rot(1,2),trans.y(),
                          rot(2,0),rot(2,1),rot(2,2),trans.z(),
                          FT(0) , FT(0),  FT(0) , FT(1));
    }

    //! compose homogeneouse matrix from other matrix
    template<class FT>
    Matx44<FT> composeHom(const Matx33<FT> &m) {
        return Matx44<FT>(m(0,0),m(0,1),m(0,2),FT(0),
                          m(1,0),m(1,1),m(1,2),FT(0),
                          m(2,0),m(2,1),m(2,2),FT(0),
                          FT(0) , FT(0),  FT(0) , FT(1));
    }

    //! decompose homogeneouse matrix to trans vector and rot vector
    template<class FT>
    void decomposeHom(const Matx44<FT> &m, Vec3<FT> &trans, Vec3<FT> &rot) {
        Matx33<FT> r;
        decomposeHom(m,trans,r);
        rot = rodrigues(r);
    }

    //! decompose homogeneouse matrix to trans vector and rot matrix
    template<class FT>
    void decomposeHom(const Matx44<FT> &m, Vec3<FT> &trans, Matx33<FT> &rot) {
        trans = Vec3<FT>(m(0,3),m(1,3),m(2,3));
        decomposeHom(m,rot);
    }

    //! decompose homogeneouse matrix and  matrix
    template<class FT>
    void decomposeHom(const Matx44<FT> &m,  Matx33<FT> &m33) {
        m33 = Matx33<FT>(m(0,0),m(0,1),m(0,2),
                         m(1,0),m(1,1),m(1,2),
                         m(2,0),m(2,1),m(2,2));
    }

    //! compose homogeneouse matrix from trans vector and rot matrix
    template<class FT>
    Matx33<FT> composeHom(const Vec2<FT> &trans, const Matx22<FT> &rot) {
        return Matx33<FT>(rot(0,0),rot(0,1),trans.x(),
                          rot(1,0),rot(1,1),trans.y(),
                          FT(0) ,  FT(0),   FT(1));
    }

    //! compose homogeneouse matrix from other matrix
    template<class FT>
    Matx33<FT> composeHom(const Matx22<FT> &m) {
        return Matx33<FT>(m(0,0),m(0,1),FT(0),
                          m(1,0),m(1,1),FT(0),
                          FT(0) , FT(0),FT(1));
    }

    //! decompose homogeneouse matrix to trans vector and rot matrix
    template<class FT>
    void decomposeHom(const Matx33<FT> &m, Vec2<FT> &trans, Matx22<FT> &rot) {
        trans = Vec3<FT>(m(0,2),m(1,2));
        decomposeHom(m,rot);
    }

    //! decompose homogeneouse matrix to matrix
    template<class FT>
    void decomposeHom(const Matx33<FT> &m, Matx22<FT> &m22) {
        m22 = Matx33<FT>(m(0,0),m(0,1),
                         m(1,0),m(1,1));
    }
}

namespace Eigen {
    namespace internal {
        template<typename _Scalar, int _Rows, int _Cols>
        struct traits<lsfm::Matx<_Scalar, _Rows, _Cols> >
        {
          typedef _Scalar Scalar;
          typedef Dense StorageKind;
          typedef DenseIndex StorageIndex;
          typedef MatrixXpr XprKind;
          enum {
            RowsAtCompileTime = _Rows,
            ColsAtCompileTime = _Cols,
            MaxRowsAtCompileTime = Dynamic,
            MaxColsAtCompileTime = Dynamic,
            Flags = compute_matrix_flags<_Scalar, _Rows, _Cols, lsfm::Major<_Rows,_Cols>::type, Dynamic, Dynamic>::ret,
            CoeffReadCost = NumTraits<Scalar>::ReadCost,
            Options = lsfm::Major<_Rows,_Cols>::type,
            InnerStrideAtCompileTime = 1,
            OuterStrideAtCompileTime = (Options&RowMajor) ? ColsAtCompileTime : RowsAtCompileTime
          };
        };
    }
}

#endif
#endif


///////////////////////////////////////////////////////////////////////////////
// kernel::functional::rp_visitor.hpp                                        //
//                                                                           //
//  Copyright 2009 Erwann Rogard. Distributed under the Boost                //
//  Software License, Version 1.0. (See accompanying file                    //
//  LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt)         //
///////////////////////////////////////////////////////////////////////////////
#ifndef BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_RP_VISITOR_HPP_ER_2009
#define BOOST_STATISTICS_DETAIL_KERNEL_ESTIMATION_RP_VISITOR_HPP_ER_2009
#include <boost/type_traits/is_reference.hpp>
#include <boost/mpl/not.hpp>
#include <boost/call_traits.hpp>
#include <boost/statistics/detail/kernel/estimation/detail/mean_accumulator.hpp>
//#include <boost/statistics/detail/kernel/estimation/detail/return_if.hpp>
//#include <boost/statistics/detail/kernel/estimation/detail/range_difference.hpp>

namespace boost{
namespace statistics{
namespace detail{
namespace kernel{

// This visitor, f, keeps a data point, x0, and each time f(x) is called, it
// updates an estimate of the density at x0, p(x0), by the Rosenblatt-Parzen
// method, using a given kernel k.
//
// The estimate is the average of the kernel evaluations over the traversed
// dataset. That average is implemented by an accumulator of type A.
//
// X can be a reference which is only recommended if the x object is expensive
// to copy
template<
    typename K,
    typename X,
    typename A = typename
        statistics::detail::kernel::detail::mean_accumulator<typename K::result_type>::type
>
class rp_visitor : K{ //, addable<rp_visitor<K,X,A> >{ //
    typedef is_reference<X> is_ref_;
    public:
    typedef K kernel_type;
    typedef A accumulator_type;
    typedef typename K::result_type result_type;

    // Construct
    rp_visitor();
    rp_visitor(typename call_traits<X>::param_type);
    rp_visitor(
        K k, // passing radius calls implicit conversion
        typename call_traits<X>::param_type x
    );
    rp_visitor(
        K k,
        typename call_traits<X>::param_type,
        const accumulator_type&
    );
    rp_visitor(const rp_visitor&);
    rp_visitor& operator=(const rp_visitor&);

    // Update
    // Passing the training data x1 updates the estimator
    template<typename X1> result_type operator()(const X1& x1);

    // Access
    typename call_traits<X>::const_reference x()const;
    const A& accumulator()const;
    const result_type& normalizing_constant()const;

    result_type estimate()const;

    private:
    typename call_traits<X>::value_type x_;
    A acc_;
};

//Construction
template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(){
    BOOST_MPL_ASSERT((
        mpl::not_<is_ref_>
    ));
}

template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(K k,typename call_traits<X>::param_type x)
:K(k),x_(x),acc_(){}

template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(
    K k,
    typename call_traits<X>::param_type x,
    const A& a
):K(k),x_(x),acc_(a){}

template<typename K,typename X,typename A>
rp_visitor<K,X,A>::rp_visitor(const rp_visitor& that)
:K(static_cast<const K&>(that)),x_(that.x_),acc_(that.acc_){}

template<typename K,typename X,typename A>
typename rp_visitor<K,X,A>::rp_visitor&
rp_visitor<K,X,A>::operator=(const rp_visitor& that){
    if(&that!=this){
        BOOST_MPL_ASSERT((mpl::not_<is_ref_>));
        K::operator=(static_cast<const K&>(*that));
        x_ = that.x_;
        acc_ = that.acc_;
    }
    return *this;
}

// Evaluate
template<typename K,typename X,typename A>
template<typename X1>
typename rp_visitor<K,X,A>::result_type
rp_visitor<K,X,A>::operator()(const X1& x1){
    const K& kernel = static_cast<const K&>(*this);
    result_type t = kernel(x(),x1);
    this->acc_(t);
    return t;
}

// Access
template<typename K,typename X,typename A>
typename rp_visitor<K,X,A>::result_type
rp_visitor<K,X,A>::estimate()const{
    return accumulators::mean(
        this->accumulator()
    );
}

template<typename K,typename X,typename A>
const A&
rp_visitor<K,X,A>::accumulator()const{
    return  this->acc_;
}

template<typename K,typename X,typename A>
const typename rp_visitor<K,X,A>::result_type&
rp_visitor<K,X,A>::normalizing_constant()const{
    const K& k = static_cast<const K&>(*this);
    return k.normalizing_constant();
}

template<typename K,typename X,typename A>
typename call_traits<X>::const_reference
rp_visitor<K,X,A>::x()const{
    return this->x_;
}

}// kernel
}// detail
}// statistics
}// boost

#endif


//=======================================================================
// Copyright (c) 2013 Piotr Smulewicz
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
/**
 * @file scheduling_jobs_with_deadlines_on_a_single_machine.hpp
 * @brief
 * @author Piotr Smulewicz
 * @version 1.0
 * @date 2013-09-09
 */
#ifndef PAAL_SCHEDULING_JOBS_WITH_DEADLINES_ON_A_SINGLE_MACHINE_HPP
#define PAAL_SCHEDULING_JOBS_WITH_DEADLINES_ON_A_SINGLE_MACHINE_HPP

#include "paal/utils/functors.hpp"
#include "paal/utils/type_functions.hpp"
#include "paal/utils/assign_updates.hpp"

#include <boost/iterator/counting_iterator.hpp>
#include <boost/range/algorithm/sort.hpp>

#include <queue>
#include <vector>
#include <algorithm>
#include <utility>

namespace paal {
namespace greedy {

/**
 * @brief solve scheduling jobs on identical parallel machines problem
 * and fill start time of all jobs
 * example:
 * \snippet scheduling_jobs_with_deadlines_on_a_single_machine_example.cpp Scheduling Jobs On Single Machine Example
 * example file is
 * scheduling_jobs_with_deadlines_on_a_single_machine_example.cpp
 * @param first - jobs begin
 * @param last - jobs end
 * @param get_time
 * @param get_release_date
 * @param get_due_date
 * @param result
 * @tparam Time
 * @tparam InputIterator
 * @tparam OutputIterator
 * @tparam GetTime
 * @tparam GetDueDate
 * @tparam GetReleaseDate
 */
template <class InputIterator, class OutputIterator, class GetTime,
          class GetDueDate, class GetReleaseDate>
auto scheduling_jobs_with_deadlines_on_a_single_machine(
    const InputIterator first, const InputIterator last, GetTime get_time,
    GetReleaseDate get_release_date, GetDueDate get_due_date,
    OutputIterator result) {
    using Time = puretype(get_time(*first));
    std::vector<InputIterator> jobs;
    std::copy(boost::make_counting_iterator(first),
              boost::make_counting_iterator(last), std::back_inserter(jobs));

    auto get_due_date_from_iterator =
        utils::make_lift_iterator_functor(get_due_date);
    auto due_date_compatator = utils::make_functor_to_comparator(
        get_due_date_from_iterator, utils::greater{});
    using QueueType = std::priority_queue<
        InputIterator, std::vector<InputIterator>, decltype(due_date_compatator)>;
    QueueType active_jobs_iters(due_date_compatator);

    auto get_release_date_from_iterator =
        utils::make_lift_iterator_functor(get_release_date);
    boost::sort(jobs,
              utils::make_functor_to_comparator(get_release_date_from_iterator));
    Time start_idle = Time();
    Time longest_delay = Time();
    auto do_job = [&]() {
        auto job_iter = active_jobs_iters.top();
        active_jobs_iters.pop();
        Time start_time = std::max(start_idle, get_release_date(*job_iter));
        start_idle = start_time + get_time(*job_iter);
        assign_max(longest_delay, start_idle - get_due_date(*job_iter));
        *result = std::make_pair(job_iter, start_time);
        ++result;
    };
    for (auto job_iter : jobs) {
        while (!active_jobs_iters.empty() &&
               get_release_date(*job_iter) > start_idle)
            do_job();
        active_jobs_iters.push(job_iter);
    }
    while (!active_jobs_iters.empty()) {
        do_job();
    }

    return longest_delay;
}

} //!greedy
} //!paal

#endif // PAAL_SCHEDULING_JOBS_WITH_DEADLINES_ON_A_SINGLE_MACHINE_HPP


#include "mandel_hp.h"

#include <cassert>
#include <cstdlib>
#include <iomanip>
#include <cmath>
#include <climits>
#include <iostream>
#include <fstream>

#include <boost/thread.hpp>
#include <boost/chrono.hpp>

#include "common.h"
#include "updater.h"

using namespace std::literals;

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

FltH copysign(const FltH& a,const FltH& b)
{
	bool aneg = a<0;
	bool bneg = b<0;
	if (aneg==bneg)
		return a;
	else
		return -a;
}

template<typename Flt>
std::complex<Flt> step(const std::complex<Flt>& c, const std::complex<Flt>& z)
{
	return z*z + c;
}

template std::complex<FltH> step<FltH>(const std::complex<FltH>&, const std::complex<FltH>&);
template std::complex<FltL> step<FltL>(const std::complex<FltL>&, const std::complex<FltL>&);

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

// *************
// *** Point ***
// *************

template<typename Flt>
bool Point<Flt>::docalc(const std::complex<Flt>& c, UL cap)
{

	#ifndef NDEBUG
	if (c != orig)
	{
		auto dx = c.real() - orig.real();
		auto dy = c.imag() - orig.imag();
		using std::sqrt;
		auto dst = sqrt(dx*dx + dy*dy);
		if (dst > (0.001 * stepsize))
		{
			std::cerr << "Error: c differs from orig\n";
			std::cerr << "    c = " << c.real() << "+" << c.imag() << "i\n";
			std::cerr << " orig = " << orig.real() << "+" << orig.imag() << "i\n";
			std::cerr << " diff = " << dx << "+" << dy << "i\n";
			throw "Error: c differs from orig\n";
		}
	}
	#endif

	UL& n = iter;
	if (n <= 1)
	{
		// first bulb
		double xld = (double)z.real();
		double yld = (double)z.imag();
		double y2 = yld * yld;
		double xldp1 = xld+1.0;
		if (((xldp1*xldp1) + y2) < 0.0625)
		{
			status = Point::in;
			pixtype = pt_black;
			return true;
		}
		// main cardoid
		double xx = xld - 0.25;
		xx *= xx;
		xx += y2;
		double pp = sqrt(xx);
		if (xld < (pp - 2.0*(pp*pp) + 0.25))
		{
			status = Point::in;
			pixtype = pt_black;
			return true;
		}
	}

	bool did_smth = false;

	while (true)
	{
		if (n >= cap)
		{
			pixtype = pt_black;
			break;
		}

		auto zre = z.real();
		Flt  re_sq = zre * zre;
		auto zim = z.imag();
		Flt  im_sq = zim * zim;
		auto az2 = (double)re_sq + (double)im_sq;
		if (az2 > 4.0)
		{
			did_smth = true;
			status = Point::out;
			over = sqrtf(az2);
			pixtype = pt_normal;
			break;
		}

		z.real(re_sq-im_sq);
		auto ab = zre * zim;
		z.imag(ab+ab);
		z += c;
		++n;
	}

	return did_smth;
}

template<typename Flt>
void Point<Flt>::init(const std::complex<Flt>& c)
{
	status = Point::calc;
	iter = 1;
	z = c;
	pixtype = 0;
	#ifndef NDEBUG
	orig = c;
	#endif
}

template<typename Flt>
void Point<Flt>::col(float mod)
{
	const Point& p = *this;
	if (p.status != Point::out)
	{
		rgbval = {0,0,0};
        return;
	}
	auto x = p.iter;

	static const float pi2 = 3.1415926536f * 2;
	static const float ilg2 = 1.0f / logf(2.0f);
	static const float c000 = 0.0f / 3.0f;
	static const float c333 = 1.0f / 3.0f;
	static const float c666 = 2.0f / 3.0f;
	float f = fmodf((float)x, mod) + 1.0f;
	f /= mod;

	f -= logf(logf(p.over) * ilg2) * ilg2 / mod;
	f *= pi2;
	float r = 0.5f + 0.5f*sinf(f + pi2 * c000);
	float g = 0.5f + 0.5f*sinf(f + pi2 * c333);
	float b = 0.5f + 0.5f*sinf(f + pi2 * c666);
	int ri = clamp(int(r*256), 0, 255);
	int gi = clamp(int(g*256), 0, 255);
	int bi = clamp(int(b*256), 0, 255);
	rgbval = {(UC)ri, (UC)gi, (UC)bi};
}

template struct Point<FltH>;
template struct Point<FltL>;

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

// ***********
// *** Map ***
// ***********

template<typename Flt>
void Map<Flt>::colorize(float mod)
{
	for (UL y=0; y<new_h; ++y)
	{
		for (UL x=0; x<new_w; ++x)
		{
			get(x,y).col(mod);
		}
	}
}

template<typename Flt>
Point<Flt>& Map<Flt>::get(UL x, UL y)
{
	return points[y][x];
}

template<typename Flt>
Flt Map<Flt>::to_xpos(UL x) const
{
	assert(x<new_w);
	Flt fact = x;
	fact /= (Flt)new_w;
	fact -= (Flt)0.5;
	fact *= scale_x;
	fact += center_x;
	return fact;
}

template<typename Flt>
Flt Map<Flt>::to_ypos(UL y) const
{
	assert(y<new_h);
	Flt fact = y;
	fact /= (Flt)new_h;
	fact -= (Flt)0.5;
	fact *= scale_y;
	fact += center_y;
	return fact;
}

template<typename Flt>
void Map<Flt>::setZ(Flt z)
{
	scale_x = z;
	scale_y = z * (Flt)height / (Flt)width;
}

template<typename Flt>
void Map<Flt>::generate_init()
{
	UL x,y;
	vfx.clear();
	vfy.clear();

	new_w = width; new_h = height;

	for (y=0; y<height; ++y)
		vfy.push_back(to_ypos(y));
	for (x=0; x<width; ++x)
		vfx.push_back(to_xpos(x));

	map_all_done = false;
	points.resize(height);

	for (y=0; y<height; ++y)
	{
		points[y].resize(width);
		const Flt& yld = vfy[y];
		for (x=0; x<width; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
		}
	}
}

template<typename Flt>
auto Map<Flt>::generate(UL cap, bool display, bool extrap) -> Status
{
	bool found_one = false;
	bool did_smth = false;

	UL x,y;

	for (y=0; y<height; ++y)
	{
		if (display)
		{
			float f = 100.0f;
			f /= height;
			f *= y;
			std::cout << (int)f << "%\r" << std::flush;
		}
		const Flt& yld = vfy[y];
		for (x=0; x<width; ++x)
		{
			Point<Flt>& p = get(x,y);
			if (p.status != Point<Flt>::calc)
				continue;
			found_one = true;
			if (extrap)
			{
				UL w = width-1, h = height-1;
				if (x && x<w)
				{
					auto& pp = get(x-1,y);
					auto& pn = get(x+1,y);
					if ( (pp.status == Point<Flt>::out) &&
					     (pn.status == Point<Flt>::out) &&
					     (pp.iter == pn.iter) )
					{
						p.status = Point<Flt>::out;
						p.iter = pp.iter;
						p.over = (float)(pp.over + pn.over)/2.0f;
						did_smth = true;
						continue;
					}
				}
				if (y && y<h)
				{
					Point<Flt>& pp = get(x,y-1);
					Point<Flt>& pn = get(x,y+1);
					if ( (pp.status == Point<Flt>::out) &&
					     (pn.status == Point<Flt>::out) &&
					     (pp.iter == pn.iter) )
					{
						p.status = Point<Flt>::out;
						p.iter = pp.iter;
						p.over = (pp.over + pn.over)/2.0;
						did_smth = true;
						continue;
					}
				}
			}
			std::complex<Flt> c{vfx[x], yld};
			did_smth = p.docalc(c, p.iter+cap) || did_smth;
		}
	}
	if (!found_one)
		return all_done;
	else
		return did_smth ? was_updated : no_change;
}

template<typename Flt>
Image Map<Flt>::makeimage(float mod, UL upc)
{
	Image img(width, height);
	UL x,y;
	for (y=0; y<height; ++y)
	{
		for (x=0; x<width; ++x)
		{
			auto& p = get(x,y);
			if (p.status == Point<Flt>::out)
			{
				p.col(mod);
				img.PutPixel(x,y,p.rgbval);
			} else {
				bool clc = p.status==Point<Flt>::calc;
				bool ilu = upc && (p.iter<upc);
				if (clc && ilu)
					img.PutPixel(x,y,{255,255,255});
				else
					img.PutPixel(x,y,{0,0,0});
			}
		}
	}
	return img;
}

template<typename Flt>
RGB Map<Flt>::extrapolate(float x, float y)
{
	using namespace std;
	if (x<0.0f) x=0.0f; if (x>new_w) x=new_w;
	if (y<0.0f) y=0.0f; if (y>new_h) y=new_h;
	float xr = roundf(x);
	float yr = roundf(y);
	float dx = x-xr;
	float dy = y-yr;
	UL x1,x2,y1,y2;
	double fx, fy;
	if (fabs(dx) < 0.05f) {
		x1 = x2 = roundl(x);
		fx = 0.5f;
	} else {
		x1 = floorl(x);
		x2 = ceill(x);
		fx = x2-x;
	}
	if (fabs(dy) < 0.05f) {
		y1 = y2 = roundl(y);
		fy = 0.5f;
	} else {
		y1 = floorl(y);
		y2 = ceill(y);
		fy = y2-y;
	}
	RGB pix_11 = get(x1 , y1).rgbval;
	RGB pix_12 = get(x1 , y2).rgbval;
	RGB pix_21 = get(x2 , y1).rgbval;
	RGB pix_22 = get(x2 , y2).rgbval;
	RGB pix_1 = mix(pix_11, pix_12, fy);
	RGB pix_2 = mix(pix_21, pix_22, fy);
	RGB pix   = mix(pix_1,  pix_2,  fx);
	return pix;
}

template<typename Flt>
void Map<Flt>::generate_init_rest()
{
	points.resize(new_h);
	UL x,y;
	for (y=0; y<new_h; ++y)
		points[y].resize(new_w+3);

	Flt x_start = to_xpos(0);
	Flt x_stop  = to_xpos(new_w-1);
	Flt y_start = to_ypos(0);
	Flt y_stop  = to_ypos(new_h-1);
	Flt x_step  = (x_stop-x_start) / (new_w-1);
	Flt y_step  = (y_stop-y_start) / (new_h-1);

	vfx.resize(new_w); vfy.resize(new_h);
	for (x=0; x<new_w; ++x)
		vfx[x] = (x_start + x_step*x);
	for (y=0; y<new_h; ++y)
		vfy[y] = (y_start + y_step*y);
}

FltH pow(FltH m, int e)
{
	if (e==0) return FltH{1.0};
	if (e==1) return m;
	if (e<0)  return FltH{1.0} / pow(m, -e);
	FltH ret;
	mpf_pow_ui(ret.get_mpf_t(), m.get_mpf_t(), (UL)e);
	return ret;
}

template<typename Flt>
void Map<Flt>::generate_N_init(int n, bool disp)
{
	using std::pow;

	Flt tm = pow(zoom_mul, -n);

	new_w = ceill(width  * (double)tm);
	new_h = ceill(height * (double)tm);

	if (disp)
	{
		std::cout << "Old size " << width << "x" << height << std::endl;
		std::cout << "New size " << new_w << "x" << new_h << std::endl;
	}

	generate_init_rest();
}

template<typename Flt>
UL Map<Flt>::generate_N_threaded(int n, UL cap, bool display)
{
	generate_N_init(n, display);

	Updater::Init(new_h*3+4);
	if (display) Updater::Display();

	LineCache<Flt> lc[4] = {
		{ cap, display, *this },
		{ cap,   false, *this },
		{ cap,   false, *this },
		{ cap,   false, *this },
	};

	UL i = 0;
	UL num = new_h / 4;
	UL ovr = new_h - (num*4);
	UL y = 0;
	lc[0].y_start = y; y += (lc[0].y_count = num + ovr);
	lc[1].y_start = y; y += (lc[1].y_count = num);
	lc[2].y_start = y; y += (lc[2].y_count = num);
	lc[3].y_start = y; y += (lc[3].y_count = num);

	UL maxout = 0;
	boost::thread tt[4];
	for (i=1; i<4; ++i)
	{
		tt[i] = boost::thread{&LineCache<Flt>::execute, lc+i};
	}
	LineCache<Flt>::execute(lc);
	boost::chrono::nanoseconds ns{250'000};
	int joined = 1;
	while (true)
	{
		if (display) Updater::Display();

		bool j = tt[joined].try_join_for(ns);
		if (j)
		{
			++joined;
			Updater::Tick();
			if (joined >= 4) break;
		}
	}
	UL sk = 0, is = 0;
	for (i=0; i<4; ++i)
	{
		if (lc[i].eff_cap > maxout)
			maxout = lc[i].eff_cap;
		sk += lc[i].skip_count;
		is += lc[i].inskip;
	}

	Updater::Tick();
	if (display) Updater::Display();

	if (display)
		std::cout << "skipped        : " << sk << " pixels, of wich " << is << " was inside \n";

	return maxout;
}

template<typename Flt>
Image Map<Flt>::makeimage_N(int n, ModFunc mf, OSP fr)
{
	using std::pow;

	Image img(width, height);

	Flt tpmn = pow(zoom_mul, -n);

	float myw = new_w / (double)tpmn;
	float myh = new_h / (double)tpmn;

	float xstart = (new_w-myw) / 2;
	float ystart = (new_h-myh) / 2;

	float xstep = myw / width;
	float ystep = myh / height;

	Flt quot = scale_x / tpmn;
	float mod = mf((double)quot);

	if (fr) DiffReport((double)quot, mod);

	if (fr)
	{
		(*fr) << std::setprecision(20) << std::setw(25) << std::scientific << quot << " ";
		(*fr) << std::setprecision(10) << std::setw(15) << std::fixed << mod << " ";
		(*fr) << myw << "x" << myh << " ";
		(*fr) << xstart << "+" << xstep << " ";
		(*fr) << ystart << "+" << ystep << " ";
	}

	colorize(mod);

	UL x,y;
	for (y=0; y<height; ++y)
	{
		float yf = ystart + y * ystep;
		for (x=0; x<width; ++x)
		{
			double xf = xstart + x * xstep;
			img.PutPixel(x, y, extrapolate(xf, yf));
		}
	}

	return img;
}

void adjust_factor(FltL&, int) {}

int adjustments = 0;
int epsilondbl = 0;

void adjust_factor(FltH& f, int n)
{
	FltH epsilon = 0.001;
	FltH target = pow(f, n);
	FltH diff = abs(0.5f - target);

	while (true)
	{
		while (true)
		{
			FltH up = f + epsilon;
			if (up == f) return;
			FltH new_target = pow(up, n);
			FltH new_diff = abs(0.5f - new_target);
			if (new_diff<diff)
			{
				++adjustments;
				f = up;
				target = new_target;
				diff = new_diff;
				continue;
			}
			break;
		}
		while (true)
		{
			FltH dn = f - epsilon;
			if (dn == f) return;
			FltH new_target = pow(dn, n);
			FltH new_diff = abs(0.5f - new_target);
			if (new_diff<diff)
			{
				++adjustments;
				f = dn;
				target = new_target;
				diff = new_diff;
				continue;
			}
			break;
		}
		epsilon /= 2;
		++epsilondbl;
	}
}

template<typename Flt>
void Map<Flt>::setup_dbl(Flt target, MultiLogger& logger)
{
	using std::pow;
	double n = std::log(0.5) / std::log((double)target);
	count_dlb = std::roundl(n);
	Flt factor = std::pow(0.5, 1.0/n);

	logger << std::setprecision(100) << std::fixed;
	logger << "new factor     : " << factor << std::endl;
	adjust_factor(factor, count_dlb);
	logger << "adj. factor    : " << factor << std::endl;
	logger << "new count      : " << count_dlb << std::endl;
	FltH new_target = pow(factor, count_dlb);
	logger << "new doubling   : " << new_target << std::endl;

	logger << "adj / dbl      : " << adjustments << " / " << epsilondbl << std::endl;

	zoom_mul = factor;
	width  = (width  >> 2) << 2;
	height = (height >> 2) << 2;
	new_w = width  * 2;
	new_h = height * 2;
	logger << "adjusted size  : " << width << "x" << height << std::endl;
	logger << "new size       : " << new_w << "x" << new_h << std::endl;

	generate_init_rest();

	for (UL y=0; y<new_h; ++y)
	{
		const Flt& yld = vfy[y];
		for (UL x=0; x<new_w; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
		}
	}

}

template<typename Flt>
Image Map<Flt>::dbl_makefull(UL cap)
{
	std::vector<Scanline<Flt>> saved;
	saved.swap(points);

	generate_init_rest();

	for (UL y=0; y<new_h; ++y)
	{
		const Flt& yld = vfy[y];
		for (UL x=0; x<new_w; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
			get(x,y).docalc({xld, yld}, cap);
		}
	}

	using std::swap;
	swap(width, new_w); swap(height, new_h);
	auto img = makeimage(350,INT_MAX);
	swap(width, new_w); swap(height, new_h);

	saved.swap(points);

	return img;
}

template<typename Flt>
int Map<Flt>::generate_dbl(UL cap, bool first, bool display, MultiLogger& logger)
{

	#ifndef NDEBUG

	Updater::Init(new_h*3+1);
	if (display) Updater::Display();

	LineCache<Flt> lc = { cap, true, *this, first };
	lc.y_start = 0;
	lc.y_count = new_h;

	LineCache<Flt>::execute_dbl(&lc);

	UL sk = lc.skip_count;
	UL is = lc.inskip;
	UL maxout = lc.eff_cap;

	#else

	if (first)
		Updater::Init(new_h*3+4);
	else
		Updater::Init(new_h*2+4);
	if (display) Updater::Display();

	LineCache<Flt> lc[4] = {
		{ cap,  display, *this, first },
		{ cap,   false, *this, first },
		{ cap,   false, *this, first },
		{ cap,   false, *this, first },
	};

	UL i = 0;
	UL num = new_h / 4;
	UL ovr = new_h - (num*4);
	UL y = 0;
	lc[0].y_start = y; y += (lc[0].y_count = num + ovr);
	lc[1].y_start = y; y += (lc[1].y_count = num);
	lc[2].y_start = y; y += (lc[2].y_count = num);
	lc[3].y_start = y; y += (lc[3].y_count = num);

	UL maxout = 0;
	boost::thread tt[4];
	for (i=1; i<4; ++i)
	{
		tt[i] = boost::thread{&LineCache<Flt>::execute_dbl, lc+i};
	}

	LineCache<Flt>::execute_dbl(lc);

	boost::chrono::nanoseconds ns{250'000};
	int joined = 1;
	while (true)
	{
		if (display) Updater::Display();

		bool j = tt[joined].try_join_for(ns);
		if (j)
		{
			++joined;
			Updater::Tick();
			if (joined >= 4) break;
		}
	}

	UL sk = 0, is = 0;
	for (i=0; i<4; ++i)
	{
		if (lc[i].eff_cap > maxout)
			maxout = lc[i].eff_cap;
		sk += lc[i].skip_count;
		is += lc[i].inskip;
	}

	#endif

	Updater::Tick();
	if (display) Updater::Display();

	logger << "skipped        : " << sk << " pixels, of wich " << is << " was inside \n";
	logger << "effective cap  : " << maxout << "\n";

	return count_dlb;
}

template<typename Flt>
int Map<Flt>::sh_new_xcoord(int oldx)
{
	int hw = new_w/2;
	int dfc = hw - oldx;
	return hw - 2*dfc;
}

template<typename Flt>
int Map<Flt>::sh_new_ycoord(int oldy)
{
	int hh = new_h/2;
	int dfc = hh - oldy;
	return hh - 2*dfc;
}

template<typename Flt>
void Map<Flt>::new_out(std::string fn)
{
	using std::swap;
	swap(width, new_w); swap(height, new_h);
	makeimage(350,INT_MAX).Save(fn);
	swap(width, new_w); swap(height, new_h);
}

template<typename Flt>
int Map<Flt>::shuffle_dbl()
{
	generate_init_rest();

	auto copy = points;
	for (UL y=0; y<new_h; ++y)
	{
		const Flt& yld = vfy[y];
		for (UL x=0; x<new_w; ++x)
		{
			const Flt& xld = vfx[x];
			get(x,y).init({xld, yld});
		}
	}

	int cpy = 0;
	for (int y=0; y<(int)new_h; ++y)
	{
		auto newy = sh_new_ycoord(y);
		if ((newy<0) || (newy>=(int)new_h)) continue;
		for (int x=0; x<(int)new_w; ++x)
		{
			auto newx = sh_new_xcoord(x);
			if ((newy==y) && (newx==x)) continue;
			if ((newx<0) || (newx>=(int)new_w)) continue;
			const Point<Flt>& src = copy[y][x];
			Point<Flt>& dst = get(newx,newy);
			dst = src;
			++cpy;
		}
	}

	return cpy;
}

template struct Map<FltH>;
template struct Map<FltL>;

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

// *****************
// *** LineCache ***
// *****************

template<typename Flt>
LineCache<Flt>::LineCache(UL cap, bool display, Map<Flt>& map, bool first)
	: cap(cap)
	, display(display)
	, first(first)
	, map(map)
	, vfx(map.vfx)
	, vfy(map.vfy)
{}

template<typename Flt>
void LineCache<Flt>::base_init()
{
	eff_cap = 1;
	skip_count = 0;
	inskip = 0;
	n = y_count;
	w = map.new_w;
}

template<typename Flt>
void LineCache<Flt>::init_zero()
{
	for (UL i=0; i<n; ++i)
	{
		UL y = y_start + i;
		Flt& yld = vfy[y];
		for (UL x=0; x<w; ++x)
		{
			auto& p = map.get(x,y);
			std::complex<Flt> c{vfx[x], yld};
			p.init(c);
		}
	}
}

template<typename Flt>
void LineCache<Flt>::init_lim()
{
	xlo = 0;
	xhi = map.new_w-1;
	ylo = y_start;
	yhi = y_start+n-1;
}

template<typename Flt>
void LineCache<Flt>::dc(Point<Flt>& p, const std::complex<Flt>& c)
{
	bool did = p.docalc(c, cap);
	if (did && p.iter>eff_cap)
		eff_cap = p.iter;
}

template<typename Flt>
void LineCache<Flt>::even()
{
	for (UL i=0; i<n; ++i)
	{
		Updater::Tick();
		if (display)
			Updater::Display();
		if (i%2) continue;
		UL y = y_start + i;
		Flt& yld = vfy[y];
		for (UL x=0; x<w; x+=2)
		{
			auto& p = map.get(x,y);
			std::complex<Flt> c{vfx[x], yld};
			dc(p,c);
		}
	}
}

template<typename Flt>
void LineCache<Flt>::odd()
{
	for (UL i=0; i<n; ++i)
	{
		Updater::Tick();
		if (display)
			Updater::Display();
		if (!(i%2)) continue;
		UL y = y_start + i;
		Flt& yld = vfy[y];
		for (UL x=1; x<w; x+=2)
		{
			auto& p = map.get(x,y);
			if (p.status != Point<Flt>::calc) continue;
			if (p.iter != 1) continue;
			std::complex<Flt> c{vfx[x], yld};
			dc(p,c);
		}
	}
}

template<typename Flt>
bool LineCache<Flt>::ep_xy(UL x, UL y)
{
	auto& p = map.get(x,y);
	if (p.status != Point<Flt>::calc) return false;
	if (p.iter != 1) return false;
	if (x==xlo) return false;
	if (x==xhi) return false;
	if (y==ylo) return false;
	if (y==yhi) return false;
	if (map.get(x-1,y).status != Point<Flt>::in) return false;
	if (map.get(x+1,y).status != Point<Flt>::in) return false;
	if (map.get(x,y-1).status != Point<Flt>::in) return false;
	if (map.get(x,y+1).status != Point<Flt>::in) return false;
	p.status = Point<Flt>::in;
	p.pixtype = pt_black;
	++skip_count;
	++inskip;
	return true;
}

template<typename Flt>
bool LineCache<Flt>::ep_x(UL x, UL y)
{
	auto& p = map.get(x,y);
	if (p.status != Point<Flt>::calc) return false;
	if (p.iter != 1) return false;
	if (x==xlo) return false;
	if (x==xhi) return false;
	auto& pp = map.get(x-1,y);
	if (pp.status != Point<Flt>::out) return false;
	bool pe = pp.pixtype & pt_ep_msk;
	auto& pn = map.get(x+1,y);
	if (pn.status != Point<Flt>::out) return false;
	if (pp.iter != pn.iter) return false;
	bool ne = pn.pixtype & pt_ep_msk;
	if (pe && ne) return false;
	p.status = Point<Flt>::out;
	p.iter = pp.iter;
	p.over = (pp.over + pn.over)/2.0;
	p.pixtype = pt_ep_hor;
	++skip_count;
	return true;
}

template<typename Flt>
bool LineCache<Flt>::ep_y(UL x, UL y)
{
	auto& p = map.get(x,y);
	if (p.status != Point<Flt>::calc) return false;
	if (p.iter != 1) return false;
	if (y==ylo) return false;
	if (y==yhi) return false;
	auto& pp = map.get(x,y-1);
	if (pp.status != Point<Flt>::out) return false;
	bool pe = pp.pixtype & pt_ep_msk;
	auto& pn = map.get(x,y+1);
	if (pn.status != Point<Flt>::out) return false;
	if (pp.iter != pn.iter) return false;
	bool ne = pn.pixtype & pt_ep_msk;
	if (pe && ne) return false;
	p.status = Point<Flt>::out;
	p.iter = pp.iter;
	p.over = (float)(pp.over + pn.over)/2.0f;
	p.pixtype = pt_ep_ver;
	++skip_count;
	return true;
}

template<typename Flt>
bool LineCache<Flt>::setin()
{
	bool foundin = false;

	for (UL i=0; i<n; ++i)
	{
		UL y = y_start + i;
		for (UL x=0; x<w; ++x)
		{
			auto& p = map.get(x,y);
			if (p.status ==Point<Flt>::in) foundin = true;
			if (p.status != Point<Flt>::calc) continue;
			if (p.iter < cap) continue;
			p.status = Point<Flt>::in;
			foundin = true;
		}
	}
	return foundin;
}

template<typename Flt>
void LineCache<Flt>::all()
{
	for (UL i=0; i<n; ++i)
	{
		Updater::Tick();
		if (display)
			Updater::Display();

		UL y = y_start + i;
		for (UL x=0; x<w; ++x)
		{
			auto& p = map.get(x,y);
			if (p.status != Point<Flt>::calc) continue;
			if (p.iter != 1) continue;
			std::complex<Flt> c{vfx[x], vfy[y]};
			dc(p,c);
		}
	}
}

template<typename Flt>
void LineCache<Flt>::execute(LineCache* lc)
{
	lc->base_init();
	lc->init_zero();
	lc->even();
	lc->init_lim();

	auto all_f = [&](auto f) -> void
	{
		for (UL i=0; i<lc->n; i+=1)
		{
			UL y = lc->y_start + i;
			for (UL x=0; x<lc->w; x+=1)
				f(x,y);
		}
	};

	all_f( [&](UL x, UL y) { lc->ep_x(x,y); } );
	all_f( [&](UL x, UL y) { lc->ep_y(x,y); } );

	lc->odd();

	bool foundin = lc->setin();

	if (foundin)
		all_f( [&](UL x, UL y) { lc->ep_xy(x,y); } );

	lc->all();
}

template<typename Flt>
void LineCache<Flt>::execute_dbl(LineCache* lc)
{
	lc->base_init();

	#ifndef NDEBUG
	#define IDBO(fn) lc->map.new_out(fn)
	#else
	#define IDBO(fn) (void)fn
	#endif

	IDBO("ED_0_After_BI.bmp");

	if (lc->first)
	{
		lc->even();
		IDBO("ED_1_RanEvenFrst.bmp");
	}

	lc->init_lim();

	auto all_f = [&](auto f) -> void
	{
		for (UL i=0; i<lc->n; i+=1)
		{
			UL y = lc->y_start + i;
			for (UL x=0; x<lc->w; x+=1)
				f(x,y);
		}
	};

	all_f( [&](UL x, UL y) { lc->ep_x(x,y); } );
	all_f( [&](UL x, UL y) { lc->ep_y(x,y); } );

	IDBO("ED_2_Extrapolate_X_and_Y.bmp");

	lc->odd();
	IDBO("ED_3_After_Odd.bmp");

	bool foundin = lc->setin();

	if (foundin)
	{
		IDBO("ED_4_After_Setin.bmp");
		all_f( [&](UL x, UL y) { lc->ep_xy(x,y); } );
		IDBO("ED_5_Extrapolate_XY.bmp");
	}

	lc->all();
	IDBO("ED_6_After_All.bmp");

	#undef IDBO
}

template struct LineCache<FltH>;
template struct LineCache<FltL>;

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

// ******************
// *** DiffReport ***
// ******************

namespace {
	bool first = true;
	int current;
	std::ofstream diffr;
	double prv_zf;
	float  prv_mod;
}

void DiffReport(int fr)
{
	if (first)
	{
		diffr.open("DiffReport.txt");
	} else {
		assert(fr == (current+1));
	}
	current = fr;
}

void DiffReport(double zf, float mod)
{
	if (!first)
	{
		diffr << std::setw(6) << current << " ";
		diffr << std::setprecision(30) << std::setw(35);
		diffr << std::fixed << (zf / prv_zf) << " ";
		diffr << std::setprecision(10) << std::setw(15);
		diffr << std::fixed << (mod / prv_mod) << std::endl << std::flush;
	}
	first = false;
	prv_zf  = zf;
	prv_mod = mod;
}


/*
 * Copyright (C) 2016-2019 Istituto Italiano di Tecnologia (IIT)
 *
 * This software may be modified and distributed under the terms of the
 * BSD 3-Clause license. See the accompanying LICENSE file for details.
 */

#include <BayesFilters/GPFCorrection.h>
#include <BayesFilters/utils.h>

#include <Eigen/Cholesky>

#include <exception>

using namespace bfl;
using namespace Eigen;

GPFCorrection::GPFCorrection
(
    std::unique_ptr<LikelihoodModel> likelihood_model,
    std::unique_ptr<GaussianCorrection> gaussian_correction,
    std::unique_ptr<StateModel> state_model
) noexcept :
    GPFCorrection(std::move(likelihood_model), std::move(gaussian_correction), std::move(state_model), 1)
{ }

GPFCorrection::GPFCorrection
(
    std::unique_ptr<LikelihoodModel> likelihood_model,
    std::unique_ptr<GaussianCorrection> gaussian_correction,
    std::unique_ptr<StateModel> state_model,
    unsigned int seed
) noexcept :
    likelihood_model_(std::move(likelihood_model)),
    gaussian_correction_(std::move(gaussian_correction)),
    state_model_(std::move(state_model)),
    generator_(std::mt19937_64(seed)),
    distribution_(std::normal_distribution<double>(0.0, 1.0)),
    gaussian_random_sample_([&] { return (distribution_)(generator_); })
{ }

GPFCorrection::GPFCorrection(GPFCorrection&& correction) noexcept :
    PFCorrection(std::move(correction)),
    likelihood_model_(std::move(correction.likelihood_model_)),
    gaussian_correction_(std::move(correction.gaussian_correction_)),
    state_model_(std::move(correction.state_model_)),
    generator_(std::move(correction.generator_)),
    distribution_(std::move(correction.distribution_)),
    gaussian_random_sample_(std::move(correction.gaussian_random_sample_)),
    valid_likelihood_(correction.valid_likelihood_),
    likelihood_(std::move(correction.likelihood_))
{ }

GPFCorrection& GPFCorrection::operator=(GPFCorrection&& correction) noexcept
{
    PFCorrection::operator=(std::move(correction));

    gaussian_correction_ = std::move(correction.gaussian_correction_);

    state_model_ = std::move(correction.state_model_);

    generator_ = std::move(correction.generator_);

    distribution_ = std::move(correction.distribution_);

    gaussian_random_sample_ = std::move(correction.gaussian_random_sample_);

    valid_likelihood_ = correction.valid_likelihood_;

    likelihood_ = std::move(correction.likelihood_);

    return *this;
}

MeasurementModel& GPFCorrection::getMeasurementModel() noexcept
{
    return gaussian_correction_->getMeasurementModel();
}

LikelihoodModel& GPFCorrection::getLikelihoodModel() noexcept
{
    return *likelihood_model_;
}

std::pair<bool, Eigen::VectorXd> GPFCorrection::getLikelihood()
{
    return std::make_pair(valid_likelihood_, likelihood_);
}

void GPFCorrection::correctStep(const bfl::ParticleSet& pred_particles, bfl::ParticleSet& corr_particles)
{
    /* Propagate Gaussian belief associated to each particle. */
    gaussian_correction_->correct(pred_particles, corr_particles);

    /* Sample from the proposal distribution. */
    for (std::size_t i = 0; i < pred_particles.components; i++)
    {
        corr_particles.state(i) = sampleFromProposal(corr_particles.mean(i), corr_particles.covariance(i));
    }

    /* Evaluate the likelihood. */
    std::tie(valid_likelihood_, likelihood_) = getLikelihoodModel().likelihood(getMeasurementModel(), corr_particles.state());

    if (!valid_likelihood_)
    {
        corr_particles = pred_particles;

        return;
    }

    /* Evaluate the transition probability. */
    VectorXd transition_probability = state_model_->getTransitionProbability(pred_particles.state(), corr_particles.state());

    /* Update weights in the log space.
     * w_{k} = w_{k-1} + log(likelihood) + log(transition_probability) - log(proposal_distribution)
     */
    double eps = std::numeric_limits<double>::min();
    for (std::size_t i = 0; i < pred_particles.components; i++)
    {
        corr_particles.weight(i) = pred_particles.weight(i) + std::log(likelihood_(i) + eps) + std::log(transition_probability(i) + eps) - std::log(evaluateProposal(corr_particles.state(i), corr_particles.mean(i), corr_particles.covariance(i)) + eps);
    }
}

Eigen::VectorXd GPFCorrection::sampleFromProposal(const Eigen::VectorXd& mean, const Eigen::MatrixXd& covariance)
{
    /* Evaluate the square root of the state covariance matrix using the LDL' decomposition
       (it can be used even if the covariance matrix is positive semidefinite). */
    LDLT<MatrixXd> chol_ldlt(covariance);
    MatrixXd sqrt_P = (chol_ldlt.transpositionsP() * MatrixXd::Identity(mean.size(), mean.size())).transpose() *
                       chol_ldlt.matrixL() *
                       chol_ldlt.vectorD().real().cwiseSqrt().asDiagonal();

    /* Sample i.i.d standard normal univariates. */
    VectorXd rand_vectors(mean.size());
    for (int i = 0; i < rand_vectors.size(); i++)
        rand_vectors(i) = gaussian_random_sample_();

    /* Return a sample from a normal multivariate having mean `mean` and covariance 'covariance'. */
    return mean + sqrt_P * rand_vectors;
}

double GPFCorrection::evaluateProposal
(
    const Eigen::VectorXd& state,
    const Eigen::VectorXd& mean,
    const Eigen::MatrixXd& covariance
)
{
    /* Evaluate the proposal distribution, a Gaussian centered in 'mean' and having
       covariance 'covariance', in the state 'state'. */
    return utils::multivariate_gaussian_density(state, mean, covariance).coeff(0);
}


// Copyright (c) 2022 CNES
//
// All rights reserved. Use of this source code is governed by a
// BSD-style license that can be found in the LICENSE file.
#pragma once
#include <Eigen/Core>
#include <algorithm>
#include <boost/geometry.hpp>
#include <optional>

#include "pyinterp/detail/geometry/box.hpp"
#include "pyinterp/detail/geometry/point.hpp"
#include "pyinterp/detail/math/radial_basis_functions.hpp"
#include "pyinterp/detail/math/window_functions.hpp"

namespace pyinterp::detail::geometry {

/// Index points in the Cartesian space at N dimensions.
///
/// @tparam CoordinateType The class of storage for a point's coordinates.
/// @tparam Type The type of data stored in the tree.
/// @tparam N Number of dimensions in the Cartesian space handled.
template <typename CoordinateType, typename Type, size_t N>
class RTree {
 public:
  /// Type of the point handled by this instance.
  using point_t = geometry::PointND<CoordinateType, N>;

  /// Type of distances between two points.
  using distance_t = typename boost::geometry::default_distance_result<
      point_t, geometry::PointND<CoordinateType, N>>::type;

  /// Type of query results.
  using result_t = std::pair<distance_t, Type>;

  /// Value handled by this object
  using value_t = std::pair<point_t, Type>;

  /// Spatial index used
  using rtree_t =
      boost::geometry::index::rtree<value_t, boost::geometry::index::rstar<16>>;

  /// Type of the implicit conversion between the type of coordinates and values
  using promotion_t =
      decltype(std::declval<CoordinateType>() + std::declval<Type>());

  /// Default constructor
  RTree() : tree_(new rtree_t{}) {}

  /// Default destructor
  virtual ~RTree() = default;

  /// Default copy constructor
  RTree(const RTree &) = default;

  /// Default copy assignment operator
  auto operator=(const RTree &) -> RTree & = default;

  /// Move constructor
  RTree(RTree &&) noexcept = default;

  /// Move assignment operator
  auto operator=(RTree &&) noexcept -> RTree & = default;

  /// Returns the box able to contain all values stored in the container.
  ///
  /// @returns The box able to contain all values stored in the container or an
  /// invalid box if there are no values in the container.
  virtual inline auto bounds() const
      -> std::optional<geometry::BoxND<CoordinateType, N>> {
    if (empty()) {
      return {};
    }
    return tree_->bounds();
  }

  /// Returns the number of points of this mesh
  ///
  /// @return the number of points
  [[nodiscard]] constexpr auto size() const -> size_t { return tree_->size(); }

  /// Query if the container is empty.
  ///
  /// @return true if the container is empty.
  [[nodiscard]] constexpr auto empty() const -> bool { return tree_->empty(); }

  /// Removes all values stored in the container.
  inline auto clear() -> void { tree_->clear(); }

  /// The tree is created using packing algorithm (The old data is erased before
  /// construction.)
  ///
  /// @param points
  inline auto packing(const std::vector<value_t> &points) -> void {
    *tree_ = rtree_t(points);
  }

  /// Insert new data into the search tree
  ///
  /// @param point
  inline auto insert(const value_t &value) -> void { tree_->insert(value); }

  /// Search for the K nearest neighbors of a given point.
  ///
  /// @param point Point of interest
  /// @param k The number of nearest neighbors to search.
  /// @return the k nearest neighbors:
  auto query(const point_t &point, const uint32_t k) const
      -> std::vector<result_t> {
    auto result = std::vector<result_t>();
    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&point, &result](const auto &item) {
          result.emplace_back(std::make_pair(
              boost::geometry::distance(point, item.first), item.second));
        });
    return result;
  }

  /// Search for the nearest neighbors of a given point within a radius r.
  ///
  /// @param point Point of interest
  /// @param radius distance within which neighbors are returned
  /// @return the k nearest neighbors
  auto query_ball(const point_t &point, const distance_t radius) const
      -> std::vector<result_t> {
    auto result = std::vector<result_t>();
    std::for_each(
        tree_->qbegin(boost::geometry::index::satisfies([&](const auto &item) {
          return boost::geometry::distance(item.first, point) <= radius;
        })),
        tree_->qend(), [&point, &result](const auto &item) {
          result.emplace_back(std::make_pair(
              boost::geometry::distance(point, item.first), item.second));
        });
    return result;
  }

  /// Search for the nearest K neighbors around a given point.
  ///
  /// @param point Point of interest
  /// @param k The number of nearest neighbors to search.
  /// @return the k nearest neighbors if the point is within by its
  /// neighbors.
  auto query_within(const point_t &point, const uint32_t k) const
      -> std::vector<result_t> {
    auto result = std::vector<result_t>();
    auto points = boost::geometry::model::multi_point<point_t>();
    points.reserve(k);

    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&points, &point, &result](const auto &item) {
          points.emplace_back(item.first);
          result.emplace_back(std::make_pair(
              boost::geometry::distance(point, item.first), item.second));
        });

    // Are found points located around the requested point?
    if (!boost::geometry::covered_by(
            point, boost::geometry::return_envelope<
                       boost::geometry::model::box<point_t>>(points))) {
      return {};
    }
    return result;
  }

  /// Interpolation of the value at the requested position.
  ///
  /// @param point Point of interrest
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param p the power parameter.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return a tuple containing the interpolated value and the number of
  /// neighbors used in the calculation.
  auto inverse_distance_weighting(const point_t &point, distance_t radius,
                                  uint32_t k, uint32_t p, bool within) const
      -> std::pair<distance_t, uint32_t> {
    distance_t result = 0;
    distance_t total_weight = 0;

    // We're looking for the nearest k points.
    auto nearest = within ? query_within(point, k) : query(point, k);
    uint32_t neighbors = 0;

    // For each point, the distance between the point requested and the point
    // found is calculated and the information required for the Inverse distance
    // weighting interpolation method is updated.
    for (const auto &item : nearest) {
      const auto distance = item.first;
      if (distance < 1e-6) {
        // If the user has requested a grid point, the mesh value is returned.
        return std::make_pair(item.second, k);
      }

      if (distance <= radius) {
        // If the neighbor found is within an acceptable radius it can be taken
        // into account in the calculation.
        auto wk =
            static_cast<Type>(1 / std::pow(distance, static_cast<Type>(p)));
        total_weight += wk;
        result += item.second * wk;
        ++neighbors;
      }
    }

    // Finally the interpolated value is returned if there are selected points
    // otherwise one returns an undefined value.
    return total_weight != 0
               ? std::make_pair(static_cast<distance_t>(result / total_weight),
                                neighbors)
               : std::make_pair(std::numeric_limits<distance_t>::quiet_NaN(),
                                static_cast<uint32_t>(0));
  }

  /// Search for the nearest K neighbors of a given point.
  ///
  /// @param point Point of interest
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @return A tuple containing the matrix describing the coordinates of the
  /// selected points and a vector of the values of the points. The arrays will
  /// be empty if no points are selected.
  auto nearest(const point_t &point, const distance_t radius,
               const uint32_t k) const
      -> std::tuple<Matrix<promotion_t>, Vector<promotion_t>> {
    auto coordinates = Matrix<promotion_t>(N, k);
    auto values = Vector<promotion_t>(k);
    auto jx = 0U;

    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&](const auto &item) {
          if (boost::geometry::distance(point, item.first) <= radius) {
            // If the point is not too far away, it is inserted and
            // its coordinates and value are stored.
            for (size_t ix = 0; ix < N; ++ix) {
              coordinates(ix, jx) = geometry::point::get(item.first, ix);
            }
            values(jx++) = item.second;
          }
        });

    // The arrays are resized according to the number of selected points. This
    // number can be zero.
    coordinates.conservativeResize(N, jx);
    values.conservativeResize(jx);
    return std::make_tuple(coordinates, values);
  }

  /// Search for the nearest K neighbors around a given point.
  ///
  /// @param point Point of interest
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @return A tuple containing the matrix describing the coordinates of the
  /// selected points and a vector of the values of the points. The arrays will
  /// be empty if no points are selected.
  auto nearest_within(const point_t &point, const distance_t radius,
                      const uint32_t k) const
      -> std::tuple<Matrix<promotion_t>, Vector<promotion_t>> {
    auto points = boost::geometry::model::multi_point<point_t>();
    auto coordinates = Matrix<promotion_t>(N, k);
    auto values = Vector<promotion_t>(k);
    auto jx = 0U;

    // List of selected points ()
    points.reserve(k);

    std::for_each(
        tree_->qbegin(boost::geometry::index::nearest(point, k)), tree_->qend(),
        [&](const auto &item) {
          if (boost::geometry::distance(point, item.first) <= radius) {
            // If the point is not too far away, it is inserted and
            // its coordinates and value are stored.
            points.emplace_back(item.first);
            for (size_t ix = 0; ix < N; ++ix) {
              coordinates(ix, jx) = geometry::point::get(item.first, ix);
            }
            values(jx++) = item.second;
          }
        });

    // If the point is not covered by its closest neighbors, an empty set will
    // be returned.
    if (!boost::geometry::covered_by(
            point, boost::geometry::return_envelope<
                       boost::geometry::model::box<point_t>>(points))) {
      jx = 0;
    }

    // The arrays are resized according to the number of selected points. This
    // number can be zero.
    coordinates.conservativeResize(N, jx);
    values.conservativeResize(jx);
    return std::make_tuple(coordinates, values);
  }

  /// Interpolate the value of a point using a Radial Basis Function.
  ///
  /// @param point Point of interest
  /// @param rbf The radial basis function to be used.
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return A pair containing the interpolated value and the number of
  /// neighbors used in the calculation.
  auto radial_basis_function(const point_t &point,
                             const math::RBF<promotion_t> &rbf,
                             distance_t radius, uint32_t k, bool within) const
      -> std::pair<promotion_t, uint32_t> {
    auto [coordinates, values] =
        within ? nearest_within(point, radius, k) : nearest(point, radius, k);
    if (values.size() == 0) {
      return std::make_pair(std::numeric_limits<promotion_t>::quiet_NaN(), 0);
    }
    auto xi = Eigen::Matrix<promotion_t, N, 1>();
    for (size_t ix = 0; ix < N; ++ix) {
      xi(ix, 0) = geometry::point::get(point, ix);
    }
    auto interpolated = rbf.interpolate(coordinates, values, xi);
    return std::make_pair(interpolated(0),
                          static_cast<uint32_t>(values.size()));
  }

  /// Interpolate the value of a point using a Window Function.
  ///
  /// @param point Point of interest
  /// @param wf The window function to be used.
  /// @param radius The maximum radius of the search.
  /// @param k The number of nearest neighbors to be used for calculating the
  /// interpolated value.
  /// @param within If true, the method ensures that the neighbors found are
  /// located around the point of interest. In other words, this parameter
  /// ensures that the calculated values will not be extrapolated.
  /// @return A pair containing the interpolated value and the number of
  /// neighbors used in the calculation.
  auto window_function(const point_t &point,
                       const math::WindowFunction<distance_t> &wf,
                       const distance_t arg, distance_t radius, uint32_t k,
                       bool within) const -> std::pair<distance_t, uint32_t> {
    distance_t result = 0;
    distance_t total_weight = 0;

    auto nearest = within ? query_within(point, k) : query(point, k);
    uint32_t neighbors = 0;

    for (const auto &item : nearest) {
      const auto distance = item.first;

      auto wk = wf(distance, radius, arg);
      total_weight += wk;
      result += item.second * wk;
      ++neighbors;
    }

    return total_weight != 0
               ? std::make_pair(static_cast<distance_t>(result / total_weight),
                                neighbors)
               : std::make_pair(std::numeric_limits<distance_t>::quiet_NaN(),
                                static_cast<uint32_t>(0));
  }

 protected:
  /// Geographic index used to store data and their searches.
  std::shared_ptr<rtree_t> tree_;
};

}  // namespace pyinterp::detail::geometry


#include <iostream>
#include <vector>

#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/push_relabel_max_flow.hpp>

// Graph Type with nested interior edge properties for flow algorithms
typedef boost::adjacency_list_traits<boost::vecS, boost::vecS, boost::directedS> traits;
typedef boost::adjacency_list<boost::vecS, boost::vecS, boost::directedS, boost::no_property,
    boost::property<boost::edge_capacity_t, long,
        boost::property<boost::edge_residual_capacity_t, long,
            boost::property<boost::edge_reverse_t, traits::edge_descriptor>>>> graph;

typedef traits::vertex_descriptor vertex_desc;
typedef traits::edge_descriptor edge_desc;

class edge_adder {
  graph &G;

 public:
  explicit edge_adder(graph &G) : G(G) {}

  void add_edge(int from, int to, long capacity) {
    auto c_map = boost::get(boost::edge_capacity, G);
    auto r_map = boost::get(boost::edge_reverse, G);
    const auto e = boost::add_edge(from, to, G).first;
    const auto rev_e = boost::add_edge(to, from, G).first;
    c_map[e] = capacity;
    c_map[rev_e] = 0; // reverse edge has no capacity!
    r_map[e] = rev_e;
    r_map[rev_e] = e;
  }
};

using namespace std;

void solve() {
  int cols; cin >> cols;
  int rows; cin >> rows;

  graph G;
  edge_adder adder(G);

  int targetFlow = 0;
  vertex_desc source = boost::add_vertex(G);
  vertex_desc target = boost::add_vertex(G);

  vector<vector<vertex_desc>> in(rows, vector<vertex_desc>(cols));
  vector<vector<vertex_desc>> out(rows, vector<vertex_desc>(cols));
  for (int row = 0; row < rows; ++row) {
    for (int col = 0; col < cols; ++col) {
      in[row][col] = boost::add_vertex(G);
      out[row][col] = boost::add_vertex(G);
    }
  }
  for (int row = 0; row < rows; ++row) {
    for (int col = 0; col < cols; ++col) {
      char c; cin >> c;
      bool blocked = (c == 'x');
      if (blocked) {
        continue;
      }
      bool even = (row + col) % 2 == 0;
      if (even) {
        adder.add_edge(source, in[row][col], 1);

        if (row > 0)
          adder.add_edge(in[row][col], out[row - 1][col], 1);
        if (row < rows - 1)
          adder.add_edge(in[row][col], out[row + 1][col], 1);
        if (col > 0)
          adder.add_edge(in[row][col], out[row][col - 1], 1);
        if (col < cols - 1)
          adder.add_edge(in[row][col], out[row][col + 1], 1);
      }
      else {
        adder.add_edge(out[row][col], target, 1);
      }
      ++targetFlow;
    }
  }
  if (targetFlow % 2 != 0) {
    cout << "no" << endl;
    return;
  }
  targetFlow /= 2;

  long flow = boost::push_relabel_max_flow(G, source, target);
  cout << ((flow == targetFlow) ? "yes" : "no") << endl;
}

int main() {
  ios_base::sync_with_stdio(false);
  int t; cin >> t;
  for (int i = 0; i < t; ++i) {
    solve();
  }
}


#ifndef STAN_MATH_PRIM_MAT_FUN_MULTIPLY_HPP
#define STAN_MATH_PRIM_MAT_FUN_MULTIPLY_HPP

#include <stan/math/prim/mat/fun/Eigen.hpp>
#include <stan/math/prim/arr/err/check_matching_sizes.hpp>
#include <stan/math/prim/mat/err/check_multiplicable.hpp>
#include <boost/type_traits/is_arithmetic.hpp>
#include <boost/utility/enable_if.hpp>

namespace stan {
  namespace math {

    /**
     * Return specified matrix multiplied by specified scalar.
     * @tparam R Row type for matrix.
     * @tparam C Column type for matrix.
     * @param m Matrix.
     * @param c Scalar.
     * @return Product of matrix and scalar.
     */
    template <int R, int C, typename T>
    inline
    typename boost::enable_if_c<boost::is_arithmetic<T>::value,
                                Eigen::Matrix<double, R, C> >::type
    multiply(const Eigen::Matrix<double, R, C>& m,
             T c) {
      return c * m;
    }

    /**
     * Return specified scalar multiplied by specified matrix.
     * @tparam R Row type for matrix.
     * @tparam C Column type for matrix.
     * @param c Scalar.
     * @param m Matrix.
     * @return Product of scalar and matrix.
     */
    template <int R, int C, typename T>
    inline
    typename boost::enable_if_c<boost::is_arithmetic<T>::value,
                                Eigen::Matrix<double, R, C> >::type
    multiply(T c,
             const Eigen::Matrix<double, R, C>& m) {
      return c * m;
    }

    /**
     * Return the product of the specified matrices.  The number of
     * columns in the first matrix must be the same as the number of rows
     * in the second matrix.
     * @param m1 First matrix.
     * @param m2 Second matrix.
     * @return The product of the first and second matrices.
     * @throw std::domain_error if the number of columns of m1 does not match
     *   the number of rows of m2.
     */
    template<int R1, int C1, int R2, int C2>
    inline Eigen::Matrix<double, R1, C2>
    multiply(const Eigen::Matrix<double, R1, C1>& m1,
             const Eigen::Matrix<double, R2, C2>& m2) {
      check_multiplicable("multiply",
                          "m1", m1,
                          "m2", m2);
      return m1*m2;
    }

    /**
     * Return the scalar product of the specified row vector and
     * specified column vector.  The return is the same as the dot
     * product.  The two vectors must be the same size.
     * @param rv Row vector.
     * @param v Column vector.
     * @return Scalar result of multiplying row vector by column vector.
     * @throw std::domain_error if rv and v are not the same size.
     */
    template<int C1, int R2>
    inline double multiply(const Eigen::Matrix<double, 1, C1>& rv,
                           const Eigen::Matrix<double, R2, 1>& v) {
      check_matching_sizes("multiply",
                           "rv", rv,
                           "v", v);
      return rv.dot(v);
    }

  }
}
#endif