//////////////////////////////////////////////////////////////////////////////
//
// (C) Copyright Alejandro Cabrera 2011.
// 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)
//
// See http://www.boost.org/libs/bloom_filter for documentation.
//
//////////////////////////////////////////////////////////////////////////////

#ifndef BOOST_BLOOM_FILTER_TWOHASH_DYNAMIC_BASIC_BLOOM_FILTER_HPP
#define BOOST_BLOOM_FILTER_TWOHASH_DYNAMIC_BASIC_BLOOM_FILTER_HPP 1

#include <cmath>

#include <boost/config.hpp>
#include <boost/dynamic_bitset.hpp>

#include <boost/bloom_filter/hash/default.hpp>
#include <boost/bloom_filter/hash/murmurhash3.hpp>
#include <boost/bloom_filter/detail/extenders.hpp>
#include <boost/bloom_filter/detail/twohash_apply_hash.hpp>
#include <boost/bloom_filter/detail/exceptions.hpp>

namespace boost {
  namespace bloom_filters {
    template <typename T,
	      size_t HashValues = 2,
	      size_t ExpectedInsertionCount = 0,
	      class HashFunction1 = boost_hash<T>,
	      class HashFunction2 = murmurhash3<T>,
	      typename ExtensionFunction = detail::square,
	      typename Block = size_t,
	      typename Allocator = std::allocator<Block> >// = ???
    class twohash_dynamic_basic_bloom_filter {
    public:
      typedef T value_type;
      typedef T key_type;
      typedef Block block_type;
      typedef Allocator allocator_type;
      typedef boost::dynamic_bitset<Block, Allocator> bitset_type;
      typedef HashFunction1 hash_function1_type;
      typedef HashFunction2 hash_function2_type;
      typedef ExtensionFunction extension_function_type;
      typedef twohash_dynamic_basic_bloom_filter<T,
						 HashValues,
						 ExpectedInsertionCount,
						 HashFunction1,
						 HashFunction2,
						 ExtensionFunction,
						 Block,
						 Allocator> this_type;

      static const size_t default_size = 32;

    private:
      typedef detail::twohash_apply_hash<HashValues,
					 this_type> apply_hash_type;

    public:
      //* constructors
      twohash_dynamic_basic_bloom_filter()
	: bits(default_size)
      {
      }

      explicit twohash_dynamic_basic_bloom_filter(const size_t size)
	: bits(size)
      {
      }

      template <typename InputIterator>
      twohash_dynamic_basic_bloom_filter(const InputIterator start,
				 const InputIterator end)
	: bits(std::distance(start, end) * 4)
      {
	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

      //* meta-ops
      size_t bit_capacity() const
      {
	return bits.size();
      }

      static BOOST_CONSTEXPR size_t num_hash_functions()
      {
	return HashValues;
      }

      static BOOST_CONSTEXPR size_t expected_insertion_count()
      {
	return ExpectedInsertionCount;
      }

      double false_positive_rate() const
      {
        const double n = static_cast<double>(this->bits.count());
        static const double k = static_cast<double>(HashValues);
        static const double m = static_cast<double>(this->bit_capacity());
        static const double e =
	  2.718281828459045235360287471352662497757247093699959574966;
        return std::pow(1 - std::pow(e, -k * n / m), k);
      }

      size_t count() const
      {
	return this->bits.count();
      }

      bool empty() const
      {
	return this->count() == 0;
      }

      const bitset_type&
      data() const
      {
	return this->bits;
      }

      //* core ops
      void insert(const T& t)
      {
	apply_hash_type::insert(t, bits);
      }

      template <typename InputIterator>
      void insert(const InputIterator start,
		  const InputIterator end)
      {
	for (InputIterator i = start; i != end; ++i)
	  this->insert(*i);
      }

      bool probably_contains(const T& t) const
      {
	return apply_hash_type::contains(t, bits);
      }

      void clear()
      {
	this->bits.reset();
      }

      void swap(twohash_dynamic_basic_bloom_filter& other)
      {
	twohash_dynamic_basic_bloom_filter tmp = other;
	other = *this;
	*this = tmp;
      }

      //* pairwise ops
      twohash_dynamic_basic_bloom_filter&
      operator|=(const twohash_dynamic_basic_bloom_filter& rhs)
      {
	if (this->bit_capacity() != rhs.bit_capacity())
	  throw detail::incompatible_size_exception();

	this->bits |= rhs.bits;
	return *this;
      }

      twohash_dynamic_basic_bloom_filter&
      operator&=(const twohash_dynamic_basic_bloom_filter& rhs)
      {
	if (this->bit_capacity() != rhs.bit_capacity())
	  throw detail::incompatible_size_exception();

	this->bits &= rhs.bits;
	return *this;
      }

      template<class _T, size_t _HashValues,
	       size_t _ExpectedInsertionCount,
	       class _HashFunction1,
	       class _HashFunction2,
	       class _ExtensionFunction,
	       typename _Block, class _Allocator>
      friend bool
      operator==(const twohash_dynamic_basic_bloom_filter<_T,
		                                          _HashValues,
						          _ExpectedInsertionCount,
						          _HashFunction1,
						          _HashFunction2,
		                                          _ExtensionFunction,
		                                          _Block,
		                                          _Allocator>&,
		 const twohash_dynamic_basic_bloom_filter<_T,
		                                          _HashValues,
						          _ExpectedInsertionCount,
						          _HashFunction1,
						          _HashFunction2,
		                                          _ExtensionFunction,
		                                          _Block,
		                                          _Allocator>&);

    private:
      bitset_type bits;
    };

    //* global ops
    template<class T, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1,
	     class HashFunction2, class ExtensionFunction,
	     typename Block, class Allocator>
    bool
    operator==(const twohash_dynamic_basic_bloom_filter<T,
	       	       	       	       	       	        HashValues,
	                                                ExpectedInsertionCount,
	                                                HashFunction1,
	                                                HashFunction2,
	                                                ExtensionFunction,
	                                                Block,
	                                                Allocator>& lhs,
	       const twohash_dynamic_basic_bloom_filter<T,
	                                        HashValues,
						ExpectedInsertionCount,
						HashFunction1,
						HashFunction2,
	                                        ExtensionFunction,
	                                        Block,
	                                        Allocator>& rhs)
    {
	if (lhs.bit_capacity() != rhs.bit_capacity())
	  throw detail::incompatible_size_exception();

      return lhs.bits == rhs.bits;
    }

    template<class T, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1,
	     class HashFunction2, class ExtensionFunction,
	     typename Block, class Allocator>
    bool
    operator!=(const twohash_dynamic_basic_bloom_filter<T,
	       	       	       	       	       	        HashValues,
	                                                ExpectedInsertionCount,
	                                                HashFunction1,
	                                                HashFunction2,
	                                                ExtensionFunction,
	                                                Block,
	                                                Allocator>& lhs,
	       const twohash_dynamic_basic_bloom_filter<T,
	                                                HashValues,
						        ExpectedInsertionCount,
						        HashFunction1,
						        HashFunction2,
	                                                ExtensionFunction,
	                                                Block,
	                                                Allocator>& rhs)
    {
      return !(lhs == rhs);
    }

    template<class T, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1,
	     class HashFunction2, class ExtensionFunction,
	     typename Block, class Allocator>
    twohash_dynamic_basic_bloom_filter<T,
				       HashValues,
				       ExpectedInsertionCount,
				       HashFunction1,
				       HashFunction2, ExtensionFunction,
				       Block, Allocator>
    operator|(const twohash_dynamic_basic_bloom_filter<T,
	                                               HashValues,
	                                               ExpectedInsertionCount,
	                                               HashFunction1,
	                                               HashFunction2,
	                                               ExtensionFunction,
	                                               Block,
	                                               Allocator>& lhs,
	      const twohash_dynamic_basic_bloom_filter<T,
	                                               HashValues,
	                                               ExpectedInsertionCount,
	                                               HashFunction1,
	                                               HashFunction2,
	                                               ExtensionFunction,
	                                               Block,
	                                               Allocator>& rhs)
    {
      twohash_dynamic_basic_bloom_filter<T, HashValues,
					 ExpectedInsertionCount,
					 HashFunction1, HashFunction2,
					 ExtensionFunction,
					 Block,
					 Allocator> result(lhs);

      result |= rhs;
      return result;
    }

    template<class T, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1,
	     class HashFunction2, class ExtensionFunction,
	     typename Block, class Allocator>
    twohash_dynamic_basic_bloom_filter<T,
				       HashValues,
				       ExpectedInsertionCount,
				       HashFunction1,
				       HashFunction2, ExtensionFunction,
				       Block, Allocator>
    operator&(const twohash_dynamic_basic_bloom_filter<T,
	                                               HashValues,
	                                               ExpectedInsertionCount,
	                                               HashFunction1,
	                                               HashFunction2,
	                                               ExtensionFunction,
	                                               Block,
	                                               Allocator>& lhs,
	      const twohash_dynamic_basic_bloom_filter<T,
	                                               HashValues,
	                                               ExpectedInsertionCount,
	                                               HashFunction1,
	                                               HashFunction2,
	                                               ExtensionFunction,
	                                               Block,
	                                               Allocator>& rhs)
    {
      twohash_dynamic_basic_bloom_filter<T, HashValues,
					 ExpectedInsertionCount,
					 HashFunction1, HashFunction2,
					 ExtensionFunction,
					 Block,
					 Allocator> result(lhs);

      result &= rhs;
      return result;
    }

    template<class T, size_t Size, size_t HashValues,
	     size_t ExpectedInsertionCount,
	     class HashFunction1,
	     class HashFunction2, class ExtensionFunction,
	     typename Block, class Allocator>
    void swap(twohash_dynamic_basic_bloom_filter<T,
	                                         HashValues,
	                                         ExpectedInsertionCount,
	                                         HashFunction1,
	                                         HashFunction2,
	                                         ExtensionFunction,
	                                         Block,
	                                         Allocator>& lhs,
	      twohash_dynamic_basic_bloom_filter<T,
	                                         HashValues,
	                                         ExpectedInsertionCount,
	                                         HashFunction1,
	                                         HashFunction2,
	                                         ExtensionFunction,
	                                         Block,
	                                         Allocator>& rhs)
    {
      return lhs.swap(rhs);
    }
  } // namespace bloom_filters
} // namespace boost
#endif


#ifndef FSTCLASSIFIERSVM_H
#define FSTCLASSIFIERSVM_H

/*!======================================================================
   Feature Selection Toolbox 3 source code
   ---------------------------------------

   \file    classifier_svm.hpp
   \brief   Wraps external Support Vector Machine implementation (in LibSVM) to serve as FST3 classifier
   \author  Petr Somol (somol@utia.cas.cz) with collaborators, see Contacts at http://fst.utia.cz
   \date    March 2011
   \version 3.1.0.beta
   \note    FST3 was developed using gcc 4.3 and requires
   \note    \li Boost library (http://www.boost.org/, tested with versions 1.33.1 and 1.44),
   \note    \li (\e optionally) LibSVM (http://www.csie.ntu.edu.tw/~cjlin/libsvm/,
                tested with version 3.00)
   \note    Note that LibSVM is required for SVM related tools only,
            as demonstrated in demo12t.cpp, demo23.cpp, demo25t.cpp, demo32t.cpp, etc.

*/ /*
=========================================================================
Copyright:
  * FST3 software (with exception of any externally linked libraries)
    is copyrighted by Institute of Information Theory and Automation (UTIA),
    Academy of Sciences of the Czech Republic.
  * FST3 source codes as presented here do not contain code of third parties.
    FST3 may need linkage to external libraries to exploit its functionality
    in full. For details on obtaining and possible usage restrictions
    of external libraries follow their original sources (referenced from
    FST3 documentation wherever applicable).
  * FST3 software is available free of charge for non-commercial use.
    Please address all inquires concerning possible commercial use
    of FST3, or if in doubt, to FST3 maintainer (see http://fst.utia.cz)
  * Derivative works based on FST3 are permitted as long as they remain
    non-commercial only.
  * Re-distribution of FST3 software is not allowed without explicit
    consent of the copyright holder.
Disclaimer of Warranty:
  * FST3 software is presented "as is", without warranty of any kind,
    either expressed or implied, including, but not limited to, the implied
    warranties of merchantability and fitness for a particular purpose.
    The entire risk as to the quality and performance of the program
    is with you. Should the program prove defective, you assume the cost
    of all necessary servicing, repair or correction.
Limitation of Liability:
  * The copyright holder will in no event be liable to you for damages,
    including any general, special, incidental or consequential damages
    arising out of the use or inability to use the code (including but not
    limited to loss of data or data being rendered inaccurate or losses
    sustained by you or third parties or a failure of the program to operate
    with any other programs).
========================================================================== */

#include <boost/smart_ptr.hpp>
#include <iostream>
#include <sstream>
#include <list>
#include <cstring> // memcpy
#include "error.hpp"
#include "global.hpp"
#include "classifier.hpp"

#include "svm.h"

/*============== Template parameter type naming conventions ==============
--------- Numeric types: -------------------------------------------------
DATATYPE - data sample values - usually real numbers (but may be integers
          in text processing etc.)
REALTYPE - must be real numbers - for representing intermediate results of
          calculations like mean, covariance etc.
IDXTYPE - index values for enumeration of data samples - (nonnegative) integers,
          extent depends on numbers of samples in data
DIMTYPE - index values for enumeration of features (dimensions), or classes (not
          class sizes) - (nonnegative) integers, usually lower extent than IDXTYPE,
          but be aware of expressions like _classes*_features*_features !
          in linearized representations of feature matrices for all classes
BINTYPE - feature selection marker type - represents ca. <10 different feature
          states (selected, deselected, sel./desel. temporarily 1st nested loop, 2nd...)
RETURNTYPE - criterion value: real value, but may be extended in future to support
          multiple values
--------- Class types: ---------------------------------------------------
SUBSET       - class of class type Subset
CLASSIFIER   - class implementing interface defined in abstract class Classifier
EVALUATOR    - class implementing interface defined in abstract class Sequential_Step
DISTANCE     - class implementing interface defined in abstract class Distance
DATAACCESSOR - class implementing interface defined in abstract class Data_Accessor
INTERVALCONTAINER - class of class type TIntervaller
CONTAINER    - STL container of class type TInterval
========================================================================== */

namespace FST {

// NOTE: this is the proxy to LIBSVM external library
//       therefore some FST coding conventions can not
//       be strictly kept

/*! \brief Wraps external Support Vector Machine implementation (in LibSVM) to serve as FST3 classifier
    \note clone() implementation assumes LibSVM's svm_model and svm_node is equivalent to that in LibSVM version 300

 \warning LIBSVM especially with LINEAR kernel seems to have occassional problems with certain C values on certain datasets and may freeze.
          (Other kernels are more stable but not completely immune to this problem.) This is a problem outside FST3.
*/

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
class Classifier_LIBSVM : public Classifier<RETURNTYPE,DIMTYPE,SUBSET,DATAACCESSOR> {
public:
	typedef boost::shared_ptr<DATAACCESSOR> PDataAccessor;
	typedef boost::shared_ptr<SUBSET> const PSubset;
	typedef typename DATAACCESSOR::PPattern PPattern;
	Classifier_LIBSVM();
	virtual ~Classifier_LIBSVM() {notify("Classifier_LIBSVM destructor."); cleanup();}

	void initialize(const PDataAccessor da); // must be called to pre-allocate data transfer buffers
protected:
	void allocate(); // to be called in initialize(), pre-allocates everything
	void cleanup(); // deallocates everything
public:
	void set_parameter_C(double newC) {parameters.C = newC;};
	void set_parameter_gamma(double newgamma) {parameters.gamma = newgamma;};
	void set_parameter_coef0(double newcoef0) {parameters.coef0 = newcoef0;};
	void set_kernel_type(int kernel_type) {parameters.kernel_type = kernel_type;}; // see svm.h for admissible values
	double get_parameter_C() const {return parameters.C;}
	double get_parameter_gamma() const {return parameters.gamma;}
	double get_parameter_coef0() const {return parameters.coef0;}
	int get_kernel_type() const {return parameters.kernel_type;}; // see svm.h for admissible values

	virtual bool classify(DIMTYPE &cls, const PPattern &pattern);  // classifies pattern, returns the respective class index
	virtual bool train(const PDataAccessor da, const PSubset sub); // learns from designated training part of data
	virtual bool test(RETURNTYPE &result, const PDataAccessor da); // estimates accuracy using designated test data

	bool optimize_parameters(const PDataAccessor da, const PSubset sub, const int max_points=100, const int max_throws=100, const double lgC_min=-5, const double lgC_max=9, const double lggamma_min=-15, const double lggamma_max=3, const double lgcoef0_min=-2, const double lgcoef0_max=5, std::ostream& os=std::cout);

	Classifier_LIBSVM* clone() const {return stateless_clone();} //! \note dirty workaround to enable easy usage in sequential_step ...
	Classifier_LIBSVM* sharing_clone() const {throw fst_error("Classifier_LIBSVM::sharing_clone() not supported, use Classifier_LIBSVM::stateless_clone() instead.");}
	Classifier_LIBSVM* stateless_clone() const;

	virtual std::ostream& print(std::ostream& os) const {os << "Classifier_LIBSVM(kernel="<<get_kernel_type()<<")"; return os;}
private:
	Classifier_LIBSVM(const Classifier_LIBSVM& csvm, int); // weak copy-constructor, does not copy LibSVM internal structures
protected:
	// the following to be initialized using initialize() based on dataaccessor info
	IDXTYPE _all_patterns;
	DIMTYPE _classes;
	DIMTYPE _features;
	struct svm_problem problem; // data to be stored in here
	struct svm_parameter parameters;
	struct svm_model *model;
	struct svm_node *onepattern;

	bool svm_class_weighing;

	//! Nested class to hold parameter candidates in the course of optimize_parameters() run
	class ParameterSet {
	public:
		ParameterSet(): _crit_value(0.0), _Cpar(1.0), _gamma(1.0), _coef0(1.0) {}
		ParameterSet(const RETURNTYPE crit_value, const double C, const double gamma, const double coef0) : _crit_value(crit_value), _Cpar(C), _gamma(gamma), _coef0(coef0) {}
		ParameterSet(const ParameterSet& ps) : _crit_value(ps._crit_value), _Cpar(ps._Cpar), _gamma(ps._gamma), _coef0(ps._coef0) {}
		void operator=(const ParameterSet& ps) {_crit_value=ps._crit_value; _Cpar=ps._Cpar; _gamma=ps._gamma; _coef0=ps._coef0;}
		RETURNTYPE _crit_value;
		double _Cpar; // _C is reserved
		double _gamma;
		double _coef0;
	};
	typedef list<ParameterSet> PARAMSETLIST;
	typename PARAMSETLIST::iterator iter;

private:
	boost::scoped_array<DIMTYPE> _index;
	DIMTYPE _subfeatures;
};

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_LIBSVM(const Classifier_LIBSVM& csvm, int) : // copy-constructor
	_all_patterns(csvm._all_patterns),
	_classes(csvm._classes),
	_features(csvm._features),
	svm_class_weighing(csvm.svm_class_weighing),
	_subfeatures(csvm._subfeatures)
{
	notify("Classifier_LIBSVM (weak) copy-constructor.");
	// does not copy LibSVM-defined structures, except for 'parameters'
	// other-than 'parameters' LibSVM structures are only pre-allocated using allocate()
	parameters.svm_type=csvm.parameters.svm_type;
	parameters.kernel_type=csvm.parameters.kernel_type;
	parameters.degree=csvm.parameters.degree;
	parameters.gamma=csvm.parameters.gamma;
	parameters.coef0=csvm.parameters.coef0;
	parameters.cache_size=csvm.parameters.cache_size;
	parameters.eps=csvm.parameters.eps;
	parameters.C=csvm.parameters.C;
	parameters.nr_weight=csvm.parameters.nr_weight;
	parameters.nu=csvm.parameters.nu;
	parameters.p=csvm.parameters.p;
	parameters.shrinking=csvm.parameters.shrinking;
	parameters.probability=csvm.parameters.probability;
	allocate();
	model=NULL;
	if(csvm.parameters.weight_label) memcpy((void *)parameters.weight_label,(void *)csvm.parameters.weight_label,(long)(_classes) * sizeof(int));
	if(csvm.parameters.weight) memcpy((void *)parameters.weight,(void *)csvm.parameters.weight,(long)(_classes) * sizeof(double));
	if(csvm._index) {_index.reset(new DIMTYPE[_features]); memcpy((void *)_index.get(),(void *)(csvm._index).get(),sizeof(DIMTYPE)*_features);}
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>* Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::stateless_clone() const
{
	Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR> *clone=new Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>(*this,(int)0);
	clone->set_cloned();
	return clone;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::Classifier_LIBSVM()
{
	notify("Classifier_LIBSVM constructor.");
	_all_patterns=0;
	_classes=0;
	_features=0;
	problem.x=NULL;
	problem.y=NULL;
	onepattern=NULL;
	parameters.weight_label=NULL;
	parameters.weight=NULL;
	model=NULL;
	_subfeatures=0;

	// default parameters
	parameters.svm_type=C_SVC;
	parameters.kernel_type=RBF; //LINEAR;
	parameters.degree=2;
	parameters.gamma=/*standard_gamma=*/1.0;
	parameters.coef0=1.0;
	parameters.cache_size=10;
	parameters.eps=0.001;
	parameters.C=/*standard_C=*/1.0;
	parameters.nr_weight=0;
	parameters.weight_label=NULL; // just sharing a pointer
	parameters.weight=NULL; // just sharing a pointer
	parameters.nu=1.0;
	parameters.p=1.0;
	parameters.shrinking=0;
	parameters.probability=0;
	svm_class_weighing=false;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::optimize_parameters(const PDataAccessor da, const PSubset sub, const int max_points, const int max_throws, const double lgC_min, const double lgC_max, const double lggamma_min, const double lggamma_max, const double lgcoef0_min, const double lgcoef0_max, std::ostream& os)
{
	// \note: assumes Classifier_LIBSVM::initialize() had been called for this da
	// assumes initial C, gamma, and coef0 have been set using set_parameter_*(), or that default values are OK
	// (the initial values are used as search starting point)
	/*! \warning LIBSVM LINEAR kernel seems to have problems with certain C values on certain datasets. If the optimization
	             process seemingly freezes, try narrower lgC_min and lgC_max. Especially the lower bound seems
	             important to be increased.
	*/
	assert(da);
	assert(sub);
	assert(sub->get_d()>0);
	assert(max_points>0);
	assert(max_throws>0);
	assert(lgC_max>lgC_min);
	assert(lggamma_max>lggamma_min);
	assert(lgcoef0_max>lgcoef0_min);
	const double lgCrange=lgC_max-lgC_min;
	const double lggammarange=lggamma_max-lggamma_min;
	const double lgcoef0range=lgcoef0_max-lgcoef0_min;
	PARAMSETLIST _paramlist; // _paramlist.clear();
	ParameterSet tmpset, bestset;
	// evaluate the initial (current) parameter set
	RETURNTYPE val, maxval, firstval;
	RETURNTYPE result=0.0;
	RETURNTYPE cnt=0.0;
	for(bool b=da->getFirstSplit();b==true;b=da->getNextSplit()) {
		if(!train(da,sub)) return false;
		if(!test(val,da)) return false;
		result+=val;
		cnt+=1.0;
	}
	firstval=maxval=val=result/(RETURNTYPE)cnt;
	{
		ostringstream sos;
		sos << "SVM before optimization: accuracy=" << val << ", C=" << get_parameter_C();
		switch(get_kernel_type()) {
			case LINEAR:  sos << std::endl; break;
			case RBF:     sos << ", gamma=" << get_parameter_gamma() << std::endl; break;
			case POLY:
			case SIGMOID: sos << ", gamma=" << get_parameter_gamma() << ", coef0=" << get_parameter_coef0() << std::endl; break;
		}
		syncout::print(os,sos);
	}
	_paramlist.push_back(ParameterSet(val,get_parameter_C(),get_parameter_gamma(),get_parameter_coef0()));
	for(int i=0;i<max_points;i++)
	{ // find and evaluate next parameter set candidate
		double maxdistmin=0.0;
		for(int j=0;j<max_throws;j++)
		{ // find next parameter set candidate
			tmpset._Cpar =exp(lgC_min+(((double)rand()*(double)(lgC_max-lgC_min))/(double)RAND_MAX));
			tmpset._gamma=exp(lggamma_min+(((double)rand()*(double)(lggamma_max-lggamma_min))/(double)RAND_MAX));
			tmpset._coef0=exp(lgcoef0_min+(((double)rand()*(double)(lgcoef0_max-lgcoef0_min))/(double)RAND_MAX));

			double distmin=-1.0; // potential new candidate (weighted) distance to already evaluated candidates
			for(iter=_paramlist.begin();iter!=_paramlist.end();iter++)
			{
				double dist=0.0;
				assert(get_kernel_type()==LINEAR || get_kernel_type()==POLY || get_kernel_type()==RBF || get_kernel_type()==SIGMOID);
				switch(get_kernel_type()) {
					case LINEAR:  dist=sqrt(((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)*((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)); break;
					case RBF:     dist=sqrt(((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)*((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)+((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)*((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)); break;
					case POLY:
					case SIGMOID: dist=sqrt(((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)*((log(tmpset._Cpar)-log(iter->_Cpar))/lgCrange)+((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)*((log(tmpset._gamma)-log(iter->_gamma))/lggammarange)+((log(tmpset._coef0)-log(iter->_coef0))/lgcoef0range)*((log(tmpset._coef0)-log(iter->_coef0))/lgcoef0range)); break;
				}
				// candidates more distant from all others are preferred
				// distances are weighed to penalize closeness to worse-performing candidates
				assert(iter->_crit_value>0.0);
				if(distmin<0 || dist*iter->_crit_value<distmin) distmin=dist*iter->_crit_value;
			}
			assert(distmin>=0.0);
			if(distmin>maxdistmin)
			{ // better (more distant from those already tested, esp. from the bad ones) parameter set candidate found
				maxdistmin=distmin;
				bestset=tmpset;
			}
		}
		// evaluate the chosen candidate and add it to list
		set_parameter_C(bestset._Cpar);
		set_parameter_gamma(bestset._gamma);
		set_parameter_coef0(bestset._coef0);
		assert(svm_check_parameter(&problem,&parameters)==NULL);
		result=0.0;
		cnt=0.0;
		for(bool b=da->getFirstSplit();b==true;b=da->getNextSplit()) {
			if(!train(da,sub)) return false;
			if(!test(val,da)) return false;
			result+=val;
			cnt+=1.0;
		}
		val=result/(RETURNTYPE)cnt;
		bestset._crit_value=val;
		_paramlist.push_back(bestset);
		if(true)
		{
			ostringstream sos;
			sos << i+1 << ". ";
			if(val>maxval) {maxval=val; sos << "BEST accuracy=";} else sos << "test accuracy=";
			sos << bestset._crit_value << ", C=" << get_parameter_C();
			switch(get_kernel_type()) {
				case LINEAR:  sos << std::endl; break;
				case RBF:     sos << ", gamma=" << get_parameter_gamma() << std::endl; break;
				case POLY:
				case SIGMOID: sos << ", gamma=" << get_parameter_gamma() << ", coef0=" << get_parameter_coef0() << std::endl; break;
			}
			syncout::print(os,sos);
		}
	}
	// identify the best parameter set among all tested ones
	assert(!_paramlist.empty());
	iter=_paramlist.begin();
	bestset=(*iter);
	while(iter!=_paramlist.end())
	{
		if(iter->_crit_value>bestset._crit_value) bestset=(*iter);
		iter++;
	}
	set_parameter_C(bestset._Cpar);
	set_parameter_gamma(bestset._gamma);
	set_parameter_coef0(bestset._coef0);
	{
		ostringstream sos;
		sos << "SVM after optimization: accuracy=" << bestset._crit_value << ", C=" << get_parameter_C();
		switch(get_kernel_type()) {
			case LINEAR:  sos << std::endl; break;
			case RBF:     sos << ", gamma=" << get_parameter_gamma() << std::endl; break;
			case POLY:
			case SIGMOID: sos << ", gamma=" << get_parameter_gamma() << ", coef0=" << get_parameter_coef0() << std::endl; break;
		}
		sos << "Accuracy difference=" << bestset._crit_value-firstval << std::endl << std::endl;
		syncout::print(os,sos);
	}
	return false;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::allocate()
{
	try {
		//cleanup(); - not to be called here or it would destroy original data in copy-constructor
		assert(_all_patterns>0);
		assert(_classes>1);
		assert(_features>0);

		problem.y =               (double *)          malloc(_all_patterns * sizeof(double));
		problem.x =               (struct svm_node **)malloc((long)(_all_patterns) * sizeof(struct svm_node *));
		parameters.weight_label = (int *)             malloc((long)(_classes) * sizeof(int));
		parameters.weight =       (double *)          malloc((long)(_classes) * sizeof(double));
		onepattern =              (struct svm_node *) malloc((long)(_features+1) * sizeof(struct svm_node));
		for(IDXTYPE p=0;p<_all_patterns;p++) problem.x[p]=NULL;
		// pre-allocate all problem.x[] arrays to max size, to prevent re-allocation speed efficiency problems
		// - WARNING! this leads to uneconomical use of memory
		for(IDXTYPE p=0;p<_all_patterns;p++)
			problem.x[p] = (struct svm_node *) malloc((long)(_features+1) * sizeof(struct svm_node));

		// class penalty weighing disabled
		parameters.nr_weight=0;
		problem.l = _all_patterns;
		assert(svm_check_parameter(&problem,&parameters)==NULL);
	}
	catch (...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM allocation problem.");
	}
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::initialize(const PDataAccessor da)
{
	assert(da);
	try {
		cleanup();
		_all_patterns = da->getClassSizeSum(); assert(_all_patterns>0);
		_classes = da->getNoOfClasses(); assert(_classes>1);
		_features = da->getNoOfFeatures(); assert(_features>0);
		allocate();
	}
	catch (...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM initialization problem.");
	}
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
void Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::cleanup()
{
	// NOTE: called from destructor
		if(problem.x!=NULL) {
			for(IDXTYPE p=0;p<_all_patterns;p++) if(problem.x[p]!=NULL) {free(problem.x[p]); problem.x[p]=NULL;}
			free(problem.x); problem.x=NULL; // the array only, not the actual pointers
		}
		if(problem.y!=NULL) free(problem.y); problem.y=NULL;
		if(onepattern!=NULL) free(onepattern); onepattern=NULL;
		if(parameters.weight_label!=NULL) free(parameters.weight_label); parameters.weight_label=NULL;
		if(parameters.weight!=NULL) free(parameters.weight); parameters.weight=NULL;
		if(model!=NULL) {svm_free_and_destroy_model(&model); model=NULL;}

		_all_patterns=0;
		_classes=0;
		_features=0;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::train(const PDataAccessor da, const PSubset sub)
{
	assert(da);
	assert(_classes==da->getNoOfClasses());
	assert(_features==da->getNoOfFeatures());
	assert(_all_patterns==da->getClassSizeSum());
	assert(problem.x!=NULL);
	assert(problem.y!=NULL);
	assert(onepattern!=NULL);
	assert(parameters.weight_label!=NULL);
	assert(parameters.weight!=NULL);
	assert(svm_check_parameter(&problem,&parameters)==NULL);
	try {
		// feature index buffering to accelerate data transfer
		DIMTYPE f,ftmp;
		bool b;
		if(!_index || _features<sub->get_n_raw()) _index.reset(new DIMTYPE[_features]);
		for(b=sub->getFirstFeature(f),_subfeatures=0;b==true;b=sub->getNextFeature(f),_subfeatures++) {assert(_subfeatures<_features); _index[_subfeatures]=f;}
#ifdef DEBUG
		{
			ostringstream sos;
			sos << "index: "; for(f=0;f<_subfeatures;f++) sos << _index[f] << " "; //sos << std::endl;
			syncout::print(std::cout,sos);
		}
#endif
		assert(_subfeatures==sub->get_d_raw());

		typename DATAACCESSOR::PPattern p,ptmp;
		IDXTYPE s,i;//,ifeatures;
		IDXTYPE count;
		DIMTYPE nzfeat;
		double tmp;
		const DIMTYPE da_train_loop=0; // to avoid mixup of get*Block() loops of different types

		count=0;
		for(DIMTYPE c_train=0;c_train<_classes;c_train++)
		{
			da->setClass(c_train);
			for(b=da->getFirstBlock(TRAIN,p,s,da_train_loop);b==true;b=da->getNextBlock(TRAIN,p,s,da_train_loop)) for(i=0;i<s;i++)
			{
				problem.y[count]=c_train; // current pattern class info
				nzfeat=0; // no. of nonzero features processed in current pattern
				ptmp=&p[i*_features]; // temp pointer to current pattern
				for(f=0;f<_subfeatures;f++)
				{
					ftmp=_index[f]; // (full set) feature index
					tmp=ptmp[ftmp]; // feature value
					if(tmp!=0.0) {
						problem.x[count][nzfeat].index=ftmp+1;
						problem.x[count][nzfeat].value=tmp;
						nzfeat++;
					}
				}
				problem.x[count][nzfeat].index=-1;
				count++;
			}
		}
		assert(count>0);
		problem.l=count;
		if(model!=NULL) svm_free_and_destroy_model(&model);
		// train SVM model
		model=svm_train(&problem, &parameters);
	}
	catch(...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM::train() error.");
	}
	if(model==NULL) return false;
	return true;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::classify(DIMTYPE &cls, const PPattern &pattern)  // classifies pattern, returns the respective class index
{
	assert(model!=NULL);
	assert(svm_check_parameter(&problem,&parameters)==NULL);
	try {
		// assumes feature index buffering to accelerate data transfer
		assert(_index);
		assert(_subfeatures>0 && _subfeatures<=_features);

		DIMTYPE nzfeat;
		double tmp;
		RETURNTYPE tmpval;
		DIMTYPE ftmp;

		nzfeat=0; // no. of nonzero features processed in current pattern
		for(DIMTYPE f=0;f<_subfeatures;f++)
		{
			ftmp=_index[f]; // (full set) feature index
			tmp=pattern[ftmp]; // feature value
			if(tmp!=0.0) {
				onepattern[nzfeat].index=ftmp+1;
				onepattern[nzfeat].value=tmp;
				nzfeat++;
			}
		}
		onepattern[nzfeat].index=-1;
		// now classify the pattern
		tmpval=svm_predict(model, onepattern);
		cls=(DIMTYPE)tmpval;
	}
	catch(...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM::classify() error.");
	}
	return true;
}

template<class RETURNTYPE, typename IDXTYPE, typename DIMTYPE, class SUBSET, class DATAACCESSOR>
bool Classifier_LIBSVM<RETURNTYPE,IDXTYPE,DIMTYPE,SUBSET,DATAACCESSOR>::test(RETURNTYPE &result, const PDataAccessor da)
{
	assert(da);
	assert(_classes==da->getNoOfClasses());
	assert(_features==da->getNoOfFeatures());
	assert(_all_patterns==da->getClassSizeSum());
	assert(model!=NULL);
	assert(svm_check_parameter(&problem,&parameters)==NULL);
	try {
		// assumes feature index buffering to accelerate data transfer
		assert(_index);
		assert(_subfeatures>0);

		typename DATAACCESSOR::PPattern p; //,ptmp;
		IDXTYPE s,i;
		IDXTYPE count, correct;
		DIMTYPE clstmp;
		bool b;
		const DIMTYPE da_test_loop=1; // to avoid mixup of get*Block() loops of different types

		count=0;
		correct=0;
		for(DIMTYPE c_test=0;c_test<_classes;c_test++)
		{
			da->setClass(c_test);
			for(b=da->getFirstBlock(TEST,p,s,da_test_loop);b==true;b=da->getNextBlock(TEST,p,s,da_test_loop)) for(i=0;i<s;i++)
			{
				if(!classify(clstmp,&p[i*_features])) return false;
				if(clstmp==c_test) correct++;
				count++;
			}
		}
		assert(count>0);
		result=(RETURNTYPE)correct/(RETURNTYPE)count;
#ifdef DEBUG
		{
			ostringstream sos;
			sos << " result=" << result << std::endl;
			syncout::print(std::cout,sos);
		}
#endif
	}
	catch(...) {
		cleanup();
		throw fst_error("Classifier_LIBSVM::test() error.");
	}
	return true;
}

} // namespace
#endif // FSTCLASSIFIERSVM_H ///:~


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

#ifndef __UKFOM_UKF_HPP__
#define __UKFOM_UKF_HPP__

#include <vector>
#include <algorithm>
#include <numeric>

#include <boost/bind.hpp>

#include <Eigen/Core>
#include <Eigen/LU>
#include <Eigen/Eigenvalues>

#include <Eigen/QR>

#include "lapack/cholesky.hpp"
#include "traits/dof.hpp"
#include "util.hpp"

namespace ukfom {

// import most common Eigen types
using namespace Eigen;

template <typename state>
class ukf {
	typedef ukf self;

	enum {
		n = state::DOF
	};

public:

	typedef typename state::scalar_type scalar_type;
	typedef typename state::vectorized_type vectorized_state;
	typedef Matrix<scalar_type, int(state::DOF), int(state::DOF)> cov;
	typedef std::vector<state> state_vector;

	ukf(const state &mu,
		const cov &sigma)
		: mu_(mu),
		  sigma_(sigma)
	{
	}

	template<typename ProcessModel>
	void predict(ProcessModel g, const cov &R)
	{
		predict(g, boost::bind(id<cov>, R));
	}

	template<typename ProcessModel, typename ProcessNoiseCovariance>
	void predict(ProcessModel g, ProcessNoiseCovariance R)
	{
		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, sigma_, X);

		std::transform(X.begin(), X.end(), X.begin(), g);

		mu_ = sigma_points_mean(X);

		//std::cout << "mu':" << std::endl << mu_ << std::endl;

		sigma_ = sigma_points_cov<state::DOF>(mu_, X) + R();
	}

	template<typename Measurement,
			 typename MeasurementModel,
			 typename MeasurementNoiseCovariance>
	void update(const Measurement &z,
				MeasurementModel h,
				MeasurementNoiseCovariance Q)
	{
		update(z, h, Q,
			   accept_any_mahalanobis_distance<scalar_type>);
	}

	template<typename Measurement,
			 typename MeasurementModel>
	void update(const Measurement &z,
				MeasurementModel h,
				const Eigen::Matrix<scalar_type, dof<Measurement>::value, dof<Measurement>::value> &Q)
	{
		typedef Eigen::Matrix<scalar_type, dof<Measurement>::value, dof<Measurement>::value> measurement_cov;
		update(z, h,
			   boost::bind(id<measurement_cov>, Q),
			   accept_any_mahalanobis_distance<scalar_type>);
	}

	template<typename Measurement,
			 typename MeasurementModel,
			 typename MeasurementNoiseCovariance,
			 typename MahalanobisTest>
	void update(const Measurement &z,
				MeasurementModel h,
				MeasurementNoiseCovariance Q,
				MahalanobisTest mt)
	{
		const static int measurement_rows = dof<Measurement>::value;
		typedef Measurement measurement;
		typedef Eigen::Matrix<scalar_type, measurement_rows, 1> vectorized_measurement;
		typedef Matrix<scalar_type, measurement_rows, measurement_rows> measurement_cov;
		typedef Matrix<scalar_type, state::DOF, measurement_rows> cross_cov;

		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, sigma_, X);

		std::vector<measurement> Z(X.size());
		std::transform(X.begin(), X.end(), Z.begin(), h);

		const measurement meanZ = sigma_points_mean(Z);
		const measurement_cov S = sigma_points_cov<measurement_rows>(meanZ, Z) + Q();
		const cross_cov covXZ = sigma_points_cross_cov<measurement_rows>(mu_, meanZ, X, Z);

		measurement_cov S_inverse(S.inverse());

		const cross_cov K = covXZ * S_inverse;

		const vectorized_measurement innovation = z - meanZ;

		const scalar_type mahalanobis2 = (innovation.transpose() * S_inverse * innovation)(0);

		if (mt(mahalanobis2))
		{
			sigma_ -= K * S * K.transpose();
			apply_delta(K * innovation);
		}
	}

	const state &mu() const
	{
		return mu_;
	}

	const cov &sigma() const
	{
		return sigma_;
	}

private:

	void generate_sigma_points(const state &mu,
							   const vectorized_state &delta,
							   const cov &sigma,
							   state_vector &X) const
	{
		assert(X.size() == 2 * n + 1);

		MatrixXd L(sigma.llt().matrixL());

		/*if (!L.isSPD())
		{
			std::cerr << std::endl << "sigma is not SPD:" << std::endl
					  << sigma << std::endl
					  << "---" << std::endl;
			Eigen::EigenSolver<cov> eig(sigma);
			std::cerr << "eigen values: " << eig.eigenvalues().transpose() << std::endl;
		}

		assert(L.isSPD());*/

		/*
		std::cout << ">> L" << std::endl
				  << L.getL() << std::endl
				  << "<< L" << std::endl;
		*/

		X[0] = mu + delta;
		for (std::size_t i = 1, j = 0; j < n; ++j)
		{
			//std::cout << "L.col(" << j << "): " << L.getL().col(j).transpose() << std::endl;
			X[i++] = mu + (delta + L.col(j));
			X[i++] = mu + (delta - L.col(j));
		}
		//print_sigma_points(X);
	}

	void generate_sigma_points(const state &mu,
							   const cov &sigma,
							   state_vector &X) const
	{
		generate_sigma_points(mu, vectorized_state::Zero(), sigma, X);
	}

	// manifold mean
	template<typename manifold>
	manifold
	sigma_points_mean(const std::vector<manifold> &X) const
	{
		manifold reference = X[0];
		typename manifold::vectorized_type mean_delta;
		const static std::size_t max_it = 10000;

		std::size_t i = 0;
		do {
			mean_delta.setZero();
			for (typename std::vector<manifold>::const_iterator Xi = X.begin(); Xi != X.end(); ++Xi)
			{
				mean_delta += *Xi - reference;
			}
			mean_delta /= X.size();
			reference += mean_delta;
		} while (mean_delta.norm() > 1e-6
				 && ++i < max_it);

		if (i >= max_it)
		{
			std::cerr << "ERROR: sigma_points_mean() did not converge. norm(mean_delta)=" << mean_delta.norm() << std::endl;
			assert(false);
		}

		return reference;
	}

	// vector mean
	template<int measurement_rows>
	Matrix<scalar_type, measurement_rows, 1>
	sigma_points_mean(const std::vector<Matrix<scalar_type, measurement_rows, 1> > &Z) const
	{
		typedef Matrix<scalar_type, measurement_rows, 1> measurement;

		return std::accumulate(Z.begin(), Z.end(), measurement(measurement::Zero())) / Z.size();
	}

#ifdef VECT_H_
	// MTK vector mean
	template<int measurement_rows>
	MTK::vect<measurement_rows, scalar_type>
	sigma_points_mean(const std::vector<MTK::vect<measurement_rows, scalar_type> > &Z) const
	{
		typedef MTK::vect<measurement_rows, scalar_type> measurement;

		return std::accumulate(Z.begin(), Z.end(), measurement(measurement::Zero())) / Z.size();
	}
#endif // VECT_H_

	template<int cov_size, typename T>
	Matrix<scalar_type, -1, -1>
	sigma_points_cov(const T &mean, const std::vector<T> &V) const
	{
		typedef Matrix<scalar_type, -1, -1> cov_mat;
		typedef Matrix<scalar_type, cov_size, 1> cov_col;

		cov_mat c(cov_mat::Zero(cov_size,cov_size));

		for (typename std::vector<T>::const_iterator Vi = V.begin(); Vi != V.end(); ++Vi)
		{
			cov_col d = *Vi - mean;
			c += d * d.transpose();
		}

		return 0.5 * c;
	}

	template<int measurement_rows, typename Measurement>
	Matrix<scalar_type, state::DOF, measurement_rows>
	sigma_points_cross_cov(const state &meanX,
						   const Measurement &meanZ,
						   const state_vector &X,
						   const std::vector<Measurement> &Z) const
	{
		assert(X.size() == Z.size());

		typedef Matrix<scalar_type, state::DOF, measurement_rows> cross_cov;

		cross_cov c(cross_cov::Zero());

		{
			typename state_vector::const_iterator Xi = X.begin();
			typename std::vector<Measurement>::const_iterator Zi = Z.begin();
			for (;Zi != Z.end(); ++Xi, ++Zi)
			{
				c += (*Xi - meanX) * (*Zi - meanZ).transpose();
			}
		}

		return 0.5 * c;
	}

	void apply_delta(const vectorized_state &delta)
	{
		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, delta, sigma_, X);

		mu_ = sigma_points_mean(X);
		sigma_ = sigma_points_cov<state::DOF>(mu_, X);
	}

public: state mu_;
public: cov sigma_;

	// for debugging only

	void print_sigma_points(const state_vector &X) const
	{
		std::cout << "generated sigma points:" << std::endl;
		for (typename state_vector::const_iterator Xi = X.begin(); Xi != X.end(); ++Xi)
		{
			std::cout << *Xi << std::endl << "***" << std::endl;
		}
	}

public:
	void check_sigma_points()
    {
		state_vector X(2 * n + 1);
		generate_sigma_points(mu_, sigma_, X);

		state muX = sigma_points_mean(X);

		cov sigma_test = sigma_points_cov<state::DOF>(muX, X);
		if((sigma_test - sigma_).cwise().abs().maxCoeff()>1e-6){
			std::cerr << sigma_test << "\n\n" << sigma_;
			assert(false);
		}

		if (mu_ != muX)
		{
//			std::cout << "mu_:" << mu_ << std::endl;
//			std::cout << "muX:" << muX << std::endl;
			std::cout << "norm:" << ((mu_ - muX).norm() > 0. ? ">" : "=") << std::endl;
		}
		assert (mu_ == muX);
	}

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

} // namespace ukfom
#endif // __UKFOM_UKF_HPP__


//==================================================================================================
/*!
    @file

    @Copyright 2016 Numscale SAS
    @copyright 2016 J.T.Lapreste

    Distributed under the Boost Software License, Version 1.0.
    (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
*/
//==================================================================================================
#ifndef BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_X86_SSE2_SIMD_FUNCTION_SQRT_HPP_INCLUDED
#include <boost/simd/detail/overload.hpp>

#include <boost/simd/function/simd/bitwise_cast.hpp>
#include <boost/simd/function/simd/plus.hpp>
#include <boost/simd/function/simd/divides.hpp>
#include <boost/simd/function/simd/gt.hpp>
#include <boost/simd/function/simd/if_add.hpp>
#include <boost/simd/function/simd/if_else.hpp>
#include <boost/simd/function/simd/if_one_else_zero.hpp>
#include <boost/simd/function/simd/is_gez.hpp>
#include <boost/simd/function/simd/is_gtz.hpp>
#include <boost/simd/function/simd/ge.hpp>
#include <boost/simd/function/simd/is_nez.hpp>
#include <boost/simd/function/simd/lt.hpp>
#include <boost/simd/function/simd/minus.hpp>
#include <boost/simd/function/simd/shift_right.hpp>
#include <boost/simd/function/simd/tofloat.hpp>
#include <boost/simd/function/simd/toint.hpp>
#include <boost/simd/function/simd/touint.hpp>
#include <boost/simd/constant/four.hpp>
#include <boost/simd/constant/mone.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/assert_utils.hpp>

namespace boost { namespace simd { namespace ext
{
  namespace bd =  boost::dispatch;
  namespace bs =  boost::simd;
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::double_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
       return _mm_sqrt_pd(a0);
    }
  };
  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
// TODO     BOOST_ASSERT_MSG(assert_all(is_gez(a0)), "sqrt input is negative");
      using uint_type = bd::as_integer_t<A0,unsigned>;
      return simd::bitwise_cast<A0>(sqrt( simd::bitwise_cast<uint_type>(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::int64_<A0>, bs::sse_>
                         )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
//  TODO     BOOST_ASSERT_MSG(assert_all(is_gez(a0)), "sqrt input is negative");
      return bs::toint(bs::sqrt(bs::tofloat(a0)));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint8_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na  = is_nez(a0);
      A0 n   = plus(shift_right(a0, 4), Four<A0>());
      A0 n1  = shift_right(n+a0/n, 1);

      auto ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n  = if_add( gt(n*n,a0), n, Mone<A0>());
      return n+if_one_else_zero(na);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint16_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na = is_nez(a0);
      A0 const  z1 = plus(shift_right(a0, 6), Ratio<A0, 16>());
      A0 const  z2 = plus(shift_right(a0,10), Ratio<A0, 256>());
      A0 const  C1 = Ratio<A0, 31679>();
      // choose a proper starting point for approximation
      A0 n  = if_else(lt(a0, C1), z1, z2);
      auto ok =  is_gtz(n);
      n  = if_else(ok, n, One<A0>());

      A0 n1 = if_else(ok, shift_right(n+a0/n, 1), One<A0>());

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);
      n  = if_add( gt(n*n,a0), n, Mone<A0>());

     return if_add(na, Zero<A0>(), n);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint32_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      auto na = is_nez(a0);
      A0 const z1 = plus(shift_right(a0, 6),    Ratio<A0,16>());
      A0 const z2 = plus(shift_right(a0,10),   Ratio<A0,256>());
      A0 const z3 = plus(shift_right(a0,13),  Ratio<A0,2048>());
      A0 const z4 = plus(shift_right(a0,16), Ratio<A0,16384>());
      A0 n  = if_else( gt(a0, Ratio<A0,177155824>())
                  , z4
                  , if_else( gt(a0, Ratio<A0,4084387>())
                        , z3
                        , if_else( gt(a0, Ratio<A0,31679>())
                                , z2
                                , z1
                                )
                        )
                  );
      auto ok =  is_gtz(n);
      n = if_else(ok, n, One<A0>());
      A0 n1 = if_else(ok, shift_right(n+a0/n, 1), One<A0>());

      ok = lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);
      n1 = if_else(ok, shift_right(n+a0/n, 1), n1);

      ok =  lt(n1, n);
      n  = if_else(ok, n1, n);

      A0 tmp = minus(n*minus(n, One<A0>()), One<A0>());
      n  = if_add( ge(tmp+n,a0), n, Mone<A0>());
      n =  if_add(na, Zero<A0>(), n);

      return n;
     }
  };

  BOOST_DISPATCH_OVERLOAD ( sqrt_
                          , (typename A0)
                          , bs::sse2_
                          , bs::pack_<bd::uint64_<A0>, bs::sse_>
                          )
  {
    BOOST_FORCEINLINE A0 operator() ( const A0 & a0) const BOOST_NOEXCEPT
    {
      return bs::touint(bs::sqrt(bs::tofloat(a0)));
    }
  };

} } }

#endif


#include <chrono>
#include <iostream>

#include <boost/filesystem.hpp>
#include <boost/iostreams/device/mapped_file.hpp>

#include <opencv2/highgui/highgui.hpp>

#include "gil/mat.hpp"
#include "gil/vec.hpp"
#include "poisson_serial.hpp"
#include "poisson_tbb.hpp"

#include "cl/device.hpp"
#include "cl/context.hpp"
#include "cl/memory.hpp"
#include "cl/program.hpp"
#include "cl/kernel.hpp"

const size_t kNIter = 10000;

/**
 * Finds the full path of |filename| in the working directory.
 */
boost::filesystem::path find_file(const std::string& filename) {
  boost::filesystem::path path(__FILE__);
  path.remove_filename();
  path.append(filename);
  return path;
}

/**
 * Finds the smallest possible rectangular frame that contains the |mask|'s
 * white region, in other words the minimal region we'll need to consider to
 * apply poisson blending. Returns a vector with the 4 extremums of the frame.
 */
gil::vec4<size_t> find_frame(gil::mat_cview<uint8_t> mask) {
  gil::vec4<size_t> frame = {size_t(-1), size_t(-1), 0, 0};
  for (size_t i = 2; i < mask.rows()-2; ++i) {
    auto elem_it = mask.row_begin(i)+1;
    for (size_t j = 2; j < mask.cols()-2; ++j, ++elem_it) {
      // If the pixel is white, hence should be in the frame
      if (*elem_it >= 128) {
        // If the pixel is to the left of the frame, set it as the new leftmost
        if (i <= frame[0])
          frame[0] = i - 1;

        // If the pixel is atop the frame, set it as the new topmost
        if (j <= frame[1])
          frame[1] = j - 1;

        // If the pixel is to the right of the frame, set it as the new rightmost
        if (i >= frame[2])
          frame[2] = i + 1;

        // If the pixel is under the frame, set it as the new bottommost
        if (j >= frame[3])
          frame[3] = j + 1;
      }
    }
  }
  frame[0] -= 1;
  frame[1] -= 1;
  frame[2] -= frame[0] - 2; // adjust indexes relative to the min, with a 1px border
  frame[3] -= frame[1] - 2;
  return frame;
}

/**
 * Applies poisson blending on a single process. Finds a patch by applying |mask|
 * upon |src| and blend this patch on |dst| at the corresponding region (again
 * described by applying |mask|). The result of the blending is put in the
 * output parameter |result|.
 */
void poisson_blending_serial(gil::mat_cview<uint8_t> mask,
                      gil::mat_cview<gil::vec3f> src,
                      gil::mat_cview<gil::vec3f> dst,
                      gil::mat_view<gil::vec3f> result,
                      GradientMethod method) {

  assert(src.size() == mask.size());
  assert(dst.size() == mask.size());

  // Formula applied here : for all p in the destination domain (omega)
  // |N_p| * f_p - sum[all q in (N_p intersection omega)]{f_q} =
  // sum[all q in (N_p intersection delta_omega)]{f*_q} + sum[all q in N_p]{v_pq}
  // (equation 7 of http://www.cs.virginia.edu/~connelly/class/2014/comp_photo/proj2/poisson.pdf)
  // Where N_p are the neighbooring 4 pixels to p, f_p is the intensity of the source at p,
  // delta_omega is the boundary's domain, f*_q the intensity of the destination at q
  // and v_pq is the vector guidance field's value for the point between p and q,
  // ie. v_pq = g_p - g_q, with g_{something} being the source image's value at "something"
  // Do note that we do not reuse this notation.
  gil::mat<gil::vec3f> b = make_guidance(dst, src, mask, method);
  apply_mask(mask, b); // select the part corresponding to the mask's region
  gil::mat<gil::vec3f> f(dst.size()); //Will contain the intensity of the image used as input to get f_p
  gil::mat<gil::vec3f> g(dst.size()); //Will contain the output of one iteration
  copy(dst, mask, f); //applies the mask on destination and put the output as a copy in f.
  for (int i = 0; i < kNIter; ++i) { // applying iterative method to have the value of f converge
    jacobi_iteration(f, b, mask, g); // Calculate the new value of g
    f.swap(g); // use g as an input for next iteration
  }
  result = dst;
  copy(f, mask, result); // put resulting f's area corresponding to mask in
                         // output |result| variable to return
}

/**
 * Applies poisson blending on a multiple cpu process. Finds a patch by applying
 * |mask|upon |src| and blend this patch on |dst| at the corresponding region
 * (again described by applying |mask|). The result of the blending is put in
 * the output parameter |result|.
 */
void poisson_blending_tbb(gil::mat_cview<uint8_t> mask,
                          gil::mat_cview<gil::vec3f> src,
                          gil::mat_cview<gil::vec3f> dst,
                          gil::mat_view<gil::vec3f> result,
                          GradientMethod method) {

  assert(src.size() == mask.size());
  assert(dst.size() == mask.size());

  // Formula applied here : for all p in the destination domain (omega)
  // |N_p| * f_p - sum[all q in (N_p intersection omega)]{f_q} =
  // sum[all q in (N_p intersection delta_omega)]{f*_q} + sum[all q in N_p]{v_pq}
  // (equation 7 of http://www.cs.virginia.edu/~connelly/class/2014/comp_photo/proj2/poisson.pdf)
  // Where N_p are the neighbooring 4 pixels to p, f_p is the intensity of the source at p,
  // delta_omega is the boundary's domain, f*_q the intensity of the destination at q
  // and v_pq is the vector guidance field's value for the point between p and q,
  // ie. v_pq = g_p - g_q, with g_{something} being the source image's value at "something"
  // Do note that we do not reuse this notation.

  // calculate the right side of the equation, see above. Constant across solving
  gil::mat<gil::vec3f> b = tbb_make_guidance(dst, src, mask, method);
  tbb_apply_mask(mask, b); // select the part corresponding to the mask's region
  gil::mat<gil::vec3f> f(dst.size()); //Will contain the intensity of the image used as input to get f_p
  gil::mat<gil::vec3f> g(dst.size()); //Will contain the output of one iteration
  copy(dst, mask, f); //applies the mask on destination and put the output as a copy in f.
  for (int i = 0; i < kNIter; ++i) { // applying iterative method to have the value of f converge
    tbb_jacobi_iteration(f, b, mask, g); // Calculate the new value of g
    f.swap(g); // use g as an input for next iteration
  }
  result = dst;
  copy(f, mask, result); // put resulting f's area corresponding to mask in
  // output |result| variable to return
}

// Class to be used to execute the poisson blending with OpenCL.
// In a class to compile the OpenCL program on c++ compilation
class poisson_blending_cl {
 public:
  // Constructor, builds the OpenCL program
  poisson_blending_cl()
      : device_(cl::get_devices(cl::filter::gpu())[0]),
        ctx_(device_) {
    // read and load the OpenCL program from its file
    boost::iostreams::mapped_file_source poisson_source(find_file("poisson.cl"));
    program_ = cl::program(ctx_, poisson_source.data());
    try {
      program_.build();
    } catch (...) {
      std::cout << program_.get_build_info<cl::program::BuildLog>(device_);
      return;
    }

    make_boundary_ = cl::kernel(program_, "make_boundary");
    make_guidance_ = cl::kernel(program_, "make_guidance");
    make_guidance_mixed_gradient_ = cl::kernel(program_, "make_guidance_mixed_gradient");
    make_guidance_mixed_gradient_avg_ = cl::kernel(program_, "make_guidance_mixed_gradient_avg");
    jacobi_iteration_ = cl::kernel(program_, "jacobi_iteration");
    apply_mask_ = cl::kernel(program_, "apply_mask");
  }

  /**
   * Operator calculating the poisson blending using the OpenCL program.
   * Finds a patch by applying |mask| upon |src| and blend this patch on |dst| at
   * the corresponding region (again described by applying |mask|). The result
   * of the blending is put in the output parameter |result|.
   */
  void operator()(gil::mat_cview<uint8_t> mask,
                         gil::mat_cview<gil::vec3f> src,
                         gil::mat_cview<gil::vec3f> dst,
                         gil::mat_view<gil::vec3f> result,
                         GradientMethod method) {

    // Formula applied here : for all p in the destination domain (omega)
    // |N_p| * f_p - sum[all q in (N_p intersection omega)]{f_q} =
    // sum[all q in (N_p intersection delta_omega)]{f*_q} + sum[all q in N_p]{v_pq}
    // (equation 7 of http://www.cs.virginia.edu/~connelly/class/2014/comp_photo/proj2/poisson.pdf)
    // Where N_p are the neighbooring 4 pixels to p, f_p is the intensity of the source at p,
    // delta_omega is the boundary's domain, f*_q the intensity of the destination at q
    // and v_pq is the vector guidance field's value for the point between p and q,
    // ie. v_pq = g_p - g_q, with g_{something} being the source image's value at "something"
    // Do note that we do not reuse this notation.

    // OpenCl image of the mask
    cl::image cl_mask(ctx_,
      cl::image_format{cl::channel_order::kR, cl::channel_type::kUInt8},
      cl::image_desc::make_image_2d(mask.cols(), mask.rows()),
      cl::buffer::device);

    // OpenCl image of the boundary, used to calculate the right side of the poisson equation
    cl::image cl_boundary(ctx_,
      cl::image_format{cl::channel_order::kR, cl::channel_type::kUInt8},
      cl::image_desc::make_image_2d(mask.cols(), mask.rows()),
      cl::buffer::device);

    // OpenCl image to contain the intensity values from last iteration
    cl::image cl_f(ctx_,
      cl::image_format{cl::channel_order::kRGB, cl::channel_type::kFloat},
      cl::image_desc::make_image_2d(mask.cols(), mask.rows()),
      cl::buffer::device);

    // OpenCl image to contain the result of poisson iterations
    cl::image cl_g(ctx_,
      cl::image_format{cl::channel_order::kRGB, cl::channel_type::kFloat},
      cl::image_desc::make_image_2d(mask.cols(), mask.rows()),
      cl::buffer::device);

    // OpenCl image to contain the right side of the equation
    cl::image cl_guidance(ctx_,
      cl::image_format{cl::channel_order::kRGB, cl::channel_type::kFloat},
      cl::image_desc::make_image_2d(mask.cols(), mask.rows()),
      cl::buffer::device);

    // Initialise cl_mask using the mask image data
    cl::write_image(cl_mask,
      {0, 0, 0}, {mask.cols(), mask.rows(), 1}, mask.pitch(),
      reinterpret_cast<const uint8_t*>(mask.data()))
      (ctx_.default_queue(), {}).wait();

    // Initialise cl_f using the destination image data
    cl::write_image(cl_f,
      {0, 0, 0}, {dst.cols(), dst.rows(), 1}, dst.pitch(),
      reinterpret_cast<const uint8_t*>(dst.data()))
      (ctx_.default_queue(), {}).wait();

    // Initialise cl_g using the source image data
    cl::write_image(cl_g,
      {0, 0, 0}, {src.cols(), src.rows(), 1}, src.pitch(),
      reinterpret_cast<const uint8_t*>(src.data()))
      (ctx_.default_queue(), {}).wait();

    // Initialise cl_boundary by calculating the cl_mask's boundary
    cl::invoke_kernel(make_boundary_,
      {mask.cols(), mask.rows()}, std::make_tuple(cl_mask, cl_boundary))
      (ctx_.default_queue(), {}).wait();

    // Initialise cl_guidance by calculating the right side of the poisson equation.
    // We save the event of that call's end in e1.
    cl::kernel b;
    switch (method) {
      default:
      case GradientMethod::BASE:
        b = make_guidance_;
        break;

      case GradientMethod::MAX_MIXING:
        b = make_guidance_mixed_gradient_;
        break;

      case GradientMethod::AVG_MIXING:
        b = make_guidance_mixed_gradient_avg_;
        break;
    }
    auto e1 = cl::invoke_kernel(b,
      {mask.cols(), mask.rows()},
      std::make_tuple(cl_f, cl_g, cl_mask, cl_boundary, cl_guidance))
      (ctx_.default_queue(), {});

    // We apply the cl_mask on cl_f in order to select only the relevant information
    // from the destination
    cl::invoke_kernel(apply_mask_,
      {mask.cols(), mask.rows()}, std::make_tuple(cl_mask, cl_f))
      (ctx_.default_queue(), {}).wait();

    // Using iterative method to calculate cl_g
    for (size_t i = 0; i < kNIter; ++i) {
      // Once e1 happened (guidance field complete for first iteration,
      // previous iteration for the 499 other iterations), calculate
      // a new value of intensity field based on the left side of the equation
      e1 = cl::invoke_kernel(jacobi_iteration_,
      {mask.cols(), mask.rows()},
      std::make_tuple(cl_f, cl_guidance, cl_mask, cl_g))
      (ctx_.default_queue(), {e1});
      cl_g.swap(cl_f);
    }

    result = dst;
    gil::mat<gil::vec3f> tmp(dst.size());
    // once the last iteration is done, copy the resulting cl_f into tmp matrix.
    cl::read_image(cl_f,
      {0, 0, 0}, {tmp.cols(), tmp.rows(), 1}, tmp.pitch(),
      reinterpret_cast<uint8_t*>(tmp.data()))(ctx_.default_queue(), {e1}).wait();
    copy(tmp, mask, result); // Apply mask on tmp and paste the output at the corresponding
                             // region onto result, initialised with destination
  }

 private:
  cl::device device_;
  cl::context ctx_;
  cl::program program_;
  cl::kernel make_boundary_;
  cl::kernel make_guidance_;
  cl::kernel make_guidance_mixed_gradient_;
  cl::kernel make_guidance_mixed_gradient_avg_;
  cl::kernel jacobi_iteration_;
  cl::kernel apply_mask_;
};

template <class F>
double benchmark(const F& fcn, int nb_run = 3) {
  double avg = 0;
  for (int i = 0; i < nb_run; ++i) {
    auto start = std::chrono::high_resolution_clock::now();
    fcn();
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double> diff = end-start;
    avg += diff.count();
  }
  avg /= nb_run;
  return avg;
}

std::string make_filename(const std::string& base, GradientMethod method) {
  return base + "-" + std::to_string(kNIter) + "-" +
    std::to_string(static_cast<int>(method)) + ".jpg";
}

int main(int argc, const char *argv[]) {
  using namespace std::placeholders;

  // Load images
  gil::mat<gil::vec3f> dst(gil::mat_view<gil::vec3b>(
      cv::imread(argv[1])));
  gil::mat<gil::vec3f> src(gil::mat_view<gil::vec3b>(
      cv::imread(argv[2])));
  gil::mat<uint8_t> mask(gil::mat_view<gil::vec3b>(cv::imread(argv[3])));
  GradientMethod method = static_cast<GradientMethod>(atoi(argv[4]));

  gil::mat<gil::vec3f> result = dst;
  auto frame = find_frame(mask); // limit the mask's size to the minimum needed

  // Time the serial calculation of serial poisson blending and save its output in a file
  std::cout << benchmark([&](){
    poisson_blending_serial(mask[frame], src[frame], dst[frame], result[frame], method);
  }) << std::endl;
  cv::imwrite(make_filename("result-serial", method), cv::Mat(result));

  // Time the opencl calculation of serial poisson blending and save its output in a file
  poisson_blending_cl poisson_blending_cl;
  std::cout << benchmark([&](){
    poisson_blending_cl(mask[frame], src[frame], dst[frame], result[frame], method);
  }) << std::endl;
  cv::imwrite(make_filename("result-cl", method), cv::Mat(result));

  // Time the tbb calculation of serial poisson blending and save its output in a file
  std::cout << benchmark([&](){
    poisson_blending_tbb(mask[frame], src[frame], dst[frame], result[frame], method);
  }) << std::endl;
  cv::imwrite(make_filename("result-tbb", method), cv::Mat(result));

  return 0;
}


/*
For more information, please see: http://software.sci.utah.edu

The MIT License

Copyright (c) 2012 Scientific Computing and Imaging Institute,
University of Utah.

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

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

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

//    File       : SolveInverseProblemWithTikhonov.cc
//    Author     : Moritz Dannhauer, Ayla Khan, Dan White
//    Date       : November 02th, 2012 (last update)

#include <boost/bind.hpp>
#include <boost/lexical_cast.hpp>

#include <Modules/Legacy/Inverse/SolveInverseProblemWithTikhonovImpl.h>

#include <Core/Datatypes/Matrix.h>
#include <Core/Datatypes/DenseMatrix.h>
#include <Core/Datatypes/DenseColumnMatrix.h>
#include <Core/Datatypes/SparseRowMatrix.h>
#include <Core/Datatypes/MatrixTypeConversions.h>

#include <Core/Logging/LoggerInterface.h>
#include <Core/Utils/Exception.h>

namespace BioPSE
{

using namespace SCIRun;
using namespace SCIRun::Core;
using namespace SCIRun::Core::Datatypes;
using namespace SCIRun::Core::Logging;

TikhonovAlgorithmImpl::TikhonovAlgorithmImpl(const DenseMatrixHandle& forwardMatrix,
                                             const DenseMatrixHandle& measuredData,
                                             AlgorithmChoice regularizationChoice,
                                             AlgorithmSolutionSubcase regularizationSolutionSubcase,
                                             AlgorithmResidualSubcase regularizationResidualSubcase,
                                             const DenseMatrixHandle sourceWeighting,
                                             const DenseMatrixHandle sensorWeighting,
                                             bool computeRegularizedInverse,
                                             LegacyLoggerInterface* pr)
: forwardMatrix_(forwardMatrix),
measuredData_(measuredData),
sourceWeighting_(sourceWeighting),
sensorWeighting_(sensorWeighting),
regularizationChoice_(regularizationChoice),
regularizationSolutionSubcase_(regularizationSolutionSubcase),
regularizationResidualSubcase_(regularizationResidualSubcase),
lambda_(0),
computeRegularizedInverse_(computeRegularizedInverse),
pr_(pr)
{
  //TODO: size checking here.
}

MatrixHandle TikhonovAlgorithmImpl::get_inverse_solution() const
{
  return inverseSolution_;
}

MatrixHandle TikhonovAlgorithmImpl::get_inverse_matrix() const
{
  return inverseMatrix_;
}

DenseColumnMatrixHandle TikhonovAlgorithmImpl::get_regularization_parameter() const
{
  return regularizationParameter_;
}

//! Find Corner, find the maximal curvature which corresponds to the L-curve corner
double
TikhonovAlgorithmImpl::FindCorner(const TikhonovAlgorithm::LCurveInput& input, int& lambda_index)
{
  const std::vector<double>& rho = input.rho_;
  const std::vector<double>& eta = input.eta_;
  const std::vector<double>& lambdaArray = input.lambdaArray_;
  int nLambda = input.nLambda_;

  std::vector<double> deta(nLambda);
  std::vector<double> ddeta(nLambda);
  std::vector<double> drho(nLambda);
  std::vector<double> ddrho(nLambda);
  std::vector<double> lrho(nLambda);
  std::vector<double> leta(nLambda);
  DenseColumnMatrix kapa(nLambda);

  double maxKapa = -1.0e10;
  for (int i = 0; i < nLambda; i++)
  {
    lrho[i] = std::log10(rho[i]);
    leta[i] = std::log10(eta[i]);
    if(i>0)
    {
      deta[i] = (leta[i]-leta[i-1]) / (lambdaArray[i]-lambdaArray[i-1]); // compute first derivative
      drho[i] = (lrho[i]-lrho[i-1]) / (lambdaArray[i]-lambdaArray[i-1]);
    }
    if(i>1)
    {
      ddeta[i] = (deta[i]-deta[i-1]) / (lambdaArray[i]-lambdaArray[i-1]); // compute second derivative from first
      ddrho[i] = (drho[i]-drho[i-1]) / (lambdaArray[i]-lambdaArray[i-1]);
    }
  }
  drho[0] = drho[1];
  deta[0] = deta[1];
  ddrho[0] = ddrho[2];
  ddrho[1] = ddrho[2];
  ddeta[0] = ddeta[2];
  ddeta[1] = ddeta[2];

  lambda_index = 0;
  for (int i = 0; i < nLambda; i++)
  {
    kapa[i] = std::abs((drho[i] * ddeta[i] - ddrho[i] * deta[i]) /  //compute curvature
                       std::sqrt(std::pow((deta[i]*deta[i]+drho[i]*drho[i]), 3.0)));
    if (kapa[i] > maxKapa) // find max curvature
    {
      maxKapa = kapa[i];
      lambda_index = i;
    }
  }

  return lambdaArray[lambda_index];
}

double
TikhonovAlgorithmImpl::LambdaLookup(const TikhonovAlgorithm::LCurveInput& input, double lambda, int& lambda_index, const double epsilon)
{
  const std::vector<double>& lambdaArray = input.lambdaArray_;
  int nLambda = input.nLambda_;

  for (int i = 0; i < nLambda-1; ++i)
  {
    if (i > 0 && (lambda < lambdaArray[i-1] || lambda > lambdaArray[i+1])) continue;

    double lambda_step_midpoint = std::abs(lambdaArray[i+1] - lambdaArray[i])/2;

    if (std::abs(lambda - lambdaArray[i]) <= epsilon)  // TODO: is this a reasonable comparison???
    {
      lambda_index = i;
      return lambdaArray[lambda_index];
    }

    if (std::abs(lambda - lambdaArray[i]) < lambda_step_midpoint)
    {
      lambda_index = i;
      return lambdaArray[lambda_index];
    }

    if (std::abs(lambda - lambdaArray[i+1]) < lambda_step_midpoint)
    {
      lambda_index = i+1;
      return lambdaArray[lambda_index];
    }
  }
  return -1;
}

namespace LinearAlgebra
{
  void solve_lapack(const DenseMatrix& A, const DenseColumnMatrix& b, DenseColumnMatrix& x)
  {
    x = A.lu().solve(b).eval();
  }

  class LapackError : public std::exception {};
}

void TikhonovAlgorithmImpl::run(const TikhonovAlgorithmImpl::Input& input)
{
  // TODO: use DimensionMismatch exception where appropriate
  // DIMENSION CHECK!!
  const int M = forwardMatrix_->nrows();
  const int N = forwardMatrix_->ncols();
  if (M != measuredData_->nrows())
  {
    BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Input matrix dimensions must agree."));
  }
  if (1 != measuredData_->ncols())
  {
    BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Measured data must be a vector"));
  }
  //decide used Tikhonov regularization formulation (underdetermined or overdetermined)
  //based purely on relationship of number of sensors compared to number of source reconstruction points
  //UNDERDETERMINED CASE
  if ( ((M < N) && (regularizationChoice_ == automatic)) || (regularizationChoice_ == underdetermined))
  {
    //.........................................................................
    // OPERATE ON DATA:
    // Compute X = R * R^T * A^T (A * R * R^T * A^T + LAMBDA * LAMBDA * C * C^T) * Y
    //.........................................................................
    DenseMatrix ARRtrAtr, RRtrAtr;
    double lambda=0, lambda_sq=0;
    DenseMatrix forward_transpose = forwardMatrix_->transpose();
    DenseMatrix regMat;
    #ifdef SCIRUN4_CODE_TO_BE_CONVERTED_LATER
    SparseRowMatrixHandle sourceWeighting_sparse;
    #endif
    DenseColumnMatrix solution(M);
    DenseColumnMatrix RRtrAtrsolution(N);

    const DenseMatrixHandle measuredDataRef = measuredData_;
    if (!sourceWeighting_) //check if a Source Space Weighting Matrix exist?
    {
      regMat = DenseMatrix::Identity(N, N);
      ARRtrAtr = *forwardMatrix_ * forward_transpose;
      RRtrAtr = forward_transpose;
    }
    else
    {
      if(((regularizationSolutionSubcase_==solution_constrained) && ((N != sourceWeighting_->nrows()) || (N != sourceWeighting_->ncols()))) ||
         ((regularizationSolutionSubcase_==solution_constrained_squared) && (N != sourceWeighting_->nrows())))
      {
        BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Solution Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of columns) !"));
      }
      regMat = *sourceWeighting_;
      #ifdef SCIRUN4_CODE_TO_BE_CONVERTED_LATER
      sourceWeighting_sparse = sourceWeighting_->sparse();
      #endif

      if (regularizationSolutionSubcase_== solution_constrained)
      {
        RRtrAtr = *sourceWeighting_ * forward_transpose;
	      auto AR = forwardMatrix_;
	      ARRtrAtr = *AR * RRtrAtr;
      }
      else if (regularizationSolutionSubcase_== solution_constrained_squared)
      {
        auto AR = *forwardMatrix_ * *sourceWeighting_;
	      auto RtrAtr = AR.transpose().eval();
        RRtrAtr = *sourceWeighting_ * RtrAtr;
	      ARRtrAtr = AR * RtrAtr;
      }
    }
    DenseMatrix CCtr;
    if (!sensorWeighting_) //check if a Sensor Space (Noise Covariance) Weighting Matrix exist?
    {
      CCtr = DenseMatrix::Identity(M, M);
    }
    else
    {
      if (((regularizationResidualSubcase_ == residual_constrained) && ((M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()))) ||
          ((regularizationResidualSubcase_ == residual_constrained_squared) && (M != sensorWeighting_->nrows())))
      {
        BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Data Residual Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of rows) !"));
      }
      else
      {
        if (regularizationResidualSubcase_ == residual_constrained)
          CCtr = *sensorWeighting_;
        else
          if ( regularizationResidualSubcase_ == residual_constrained_squared )
            CCtr = *sensorWeighting_ * sensorWeighting_->transpose();
      }
    }

    DenseMatrix regForMatrix(M, M);

    //Get Regularization parameter(s) : Lambda
    if ((input.regMethod_ == "single") || (input.regMethod_ == "slider"))
    {
      if (input.regMethod_ == "single")
      {
        // Use single fixed lambda value, entered in UI
        lambda = input.lambdaFromTextEntry_;
      }
      else if (input.regMethod_ == "slider")
      {
        // Use single fixed lambda value, select via slider
        lambda = input.lambdaSlider_;
      }
    }
    else if (input.regMethod_ == "lcurve")
    {
      const int nLambda = input.lambdaCount_;  //declare needed variables

      std::vector<double> lambdaArray(nLambda, 0.0);
      std::vector<double> rho(nLambda, 0.0);
      std::vector<double> eta(nLambda, 0.0);

      lambdaArray[0] = input.lambdaMin_;
      const double lam_step = pow(10.0, log10(input.lambdaMax_ / input.lambdaMin_) / (nLambda-1));

      DenseColumnMatrix Ax(M);
      DenseColumnMatrix Rx(N);

      int lambda_index = 0;
      const DenseMatrix &forwardMatrixRef = *forwardMatrix_;

      int nr_rows = CCtr.nrows();
      int nr_cols = CCtr.ncols();
      const int NR_BOUNDS_CHECK = nr_rows * nr_cols;

      double* AtrA = ARRtrAtr.data();
      double* LLtr = CCtr.data();

      for (int j = 0; j < nLambda; j++)
      {
        if (j)
        {
          lambdaArray[j] = lambdaArray[j-1] * lam_step;
        }

        lambda_sq = lambdaArray[j] * lambdaArray[j]; //generate current lambda

        double* rm = regForMatrix.data();

        for (int i = 0; i < NR_BOUNDS_CHECK; i++)
        {
          rm[i] = AtrA[i] + lambda_sq * LLtr[i];
        }

        try
        {
          LinearAlgebra::solve_lapack(regForMatrix, *measuredDataRef, solution);
        }
        catch (LinearAlgebra::LapackError&)
        {
          const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda Range ''From'' value for the L-Curve calculation.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }
        catch(DimensionMismatch&)
        {
          const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }

        RRtrAtrsolution = RRtrAtr * solution;

        if (sourceWeighting_)
        {
          if (RRtrAtrsolution.nrows() == sourceWeighting_->ncols())
            Rx = *sourceWeighting_ * RRtrAtrsolution;
          else if  (RRtrAtrsolution.nrows() == sourceWeighting_->nrows())
            Rx = sourceWeighting_->transpose() * RRtrAtrsolution;
          else
          {
            const std::string errorMessage(" Solution weighting matrix unexpectedly does not fit to compute the weighted solution norm. ");
            if (pr_)
            {
              pr_->error(errorMessage);
            }
            else
            {
              std::cerr << errorMessage << std::endl;
            }
          }
        }
        else
          Rx = RRtrAtrsolution;

        // Calculate the norm of Ax-b and Rx for L curve
        Ax = forwardMatrixRef * RRtrAtrsolution;

        rho[j]=0; eta[j]=0;
        for (int k = 0; k < Ax.nrows(); k++)
        {
          double T = Ax(k) - (*measuredDataRef)(k);
          rho[j] += T*T; //norm of the data fit term
        }

        for (int k = 0; k < Rx.nrows(); k++)
        {
          double T = Rx[k];
          eta[j] += T*T; //norm of the model term
        }
        // eta and rho needed to plot the Lcurve and determine the L corner
        rho[j] = sqrt(rho[j]);
        eta[j] = sqrt(eta[j]);
      }
      boost::shared_ptr<TikhonovAlgorithm::LCurveInput> lcurveInput(new TikhonovAlgorithm::LCurveInput(rho, eta, lambdaArray, nLambda));
      lcurveInput_handle_ = lcurveInput;
      lambda = FindCorner(*lcurveInput_handle_, lambda_index);

      if (input.updateLCurveGui_)
        input.updateLCurveGui_(lambda, *lcurveInput_handle_, lambda_index);
    }
    lambda_sq = lambda * lambda;
    //compute the solution with the selected regularization parameter: lambda
    regularizationParameter_.reset(new DenseColumnMatrix(1));
    (*regularizationParameter_)(0) = lambda;
    int nr_rows = regForMatrix.nrows();
    int nr_cols = regForMatrix.ncols();

    double* AtrA = ARRtrAtr.data();
    double* RtrR = CCtr.data();
    double* rm   = regForMatrix.data();
    for (int i=0; i<(int)(nr_rows*nr_cols); i++)
    {
      rm[i] = AtrA[i] + lambda_sq * RtrR[i];
    }

    if (computeRegularizedInverse_)
    {
      if (sourceWeighting_)
      {
        inverseMatrix_.reset(new DenseMatrix(RRtrAtr * regForMatrix.inverse()));
      }
      else
      {
        inverseMatrix_.reset(new DenseMatrix(forward_transpose * regForMatrix.inverse()));
      }
      inverseSolution_.reset(new DenseMatrix(*inverseMatrix_ * *measuredData_));
    }
    else
    {
      try
      {
        LinearAlgebra::solve_lapack(regForMatrix, *measuredDataRef, solution);
      }
      catch (LinearAlgebra::LapackError&)
      {
        const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda Range ''From'' value for the L-Curve calculation.");
        if (pr_)
        {
          pr_->error(errorMessage);
        }
        else
        {
          std::cerr << errorMessage << std::endl;
        }
        throw;
      }
      catch(DimensionMismatch&)
      {
        const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
        if (pr_)
        {
          pr_->error(errorMessage);
        }
        else
        {
          std::cerr << errorMessage << std::endl;
        }
        throw;
      }

      RRtrAtrsolution = RRtrAtr * solution;
      inverseSolution_.reset(new DenseMatrix(RRtrAtrsolution));
    }

  }
  else
    //OVERDETERMINED CASE,
    //similar procedure as underdetermined case (documentation comments similar, see above)
    if ( ((regularizationChoice_ == automatic) && (M>=N)) || (regularizationChoice_==overdetermined) )
    {
      //.........................................................................
      // OPERATE ON DATA:
      // Computes X = (A^T * C^T * C * A + LAMBDA * LAMBDA * R^T * R) * A^T * C^T * C *Y
      //.........................................................................
      // calculate A^T * A
      DenseMatrix forward_transpose = forwardMatrix_->transpose();
      DenseMatrix AtrA;
      DenseMatrix CCtr;
      DenseMatrix matrixNoiseCov_transpose;

      if (!sensorWeighting_)
      //check if a Sensor Space (Noise Covariance) Weighting Matrix exist?
      {
        // For large problems, allocating this matrix causes the module to hang.
        // The CCtr matrix is only used if sensorWeighting is available.
        //
        // In general, this code really should be rewritten to use sparse matrices (SCIRun 5)
        // or have replaced with more efficient calculations
        // (Eigen may have algorithms we can use).
        //CCtr = DenseMatrix::identity(M);
        AtrA = forward_transpose * *forwardMatrix_;
      }
      else
      {
        if (((regularizationResidualSubcase_ == residual_constrained) && ((M != sensorWeighting_->nrows()) || (M != sensorWeighting_->ncols()))) ||
            ((regularizationResidualSubcase_ == residual_constrained_squared) && (M != sensorWeighting_->ncols())))
        {
          BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Data Residual Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of rows)!"));
        }

        matrixNoiseCov_transpose = sensorWeighting_->transpose();

        // regularizationResidualSubcase_ is either residual_constrained (default) or residual_constrained_squared
        if (regularizationResidualSubcase_ == residual_constrained)
          CCtr = *sensorWeighting_;
        else
          if (regularizationResidualSubcase_ == residual_constrained_squared)
            CCtr = matrixNoiseCov_transpose * *sensorWeighting_;

        AtrA = forward_transpose * CCtr * *forwardMatrix_;
      }
      // calculate R^T * R
      DenseMatrix RtrR;
      DenseMatrix regMat;

      if (!sourceWeighting_)
      {
        regMat = DenseMatrix::Identity(N, N);
        RtrR = DenseMatrix::Identity(N, N);
      }
      else
      {
        if(((regularizationSolutionSubcase_==solution_constrained) && ((N != sourceWeighting_->nrows()) || (N != sourceWeighting_->ncols()))) ||
           ((regularizationSolutionSubcase_==solution_constrained_squared) && (N != sourceWeighting_->ncols())))
        {
          BOOST_THROW_EXCEPTION(DimensionMismatch() << DimensionMismatchInfo("Solution Weighting Matrix (number of rows and columns) must fit matrix dimensions of Forward Matrix (number of columns) ! "));
        }
        regMat = *sourceWeighting_;

        if (regularizationSolutionSubcase_ == solution_constrained_squared)
          RtrR = sourceWeighting_->transpose() * regMat;
        else if (regularizationSolutionSubcase_ == solution_constrained)
          RtrR = regMat;
      }

      double lambda = 0, lambda_sq = 0;
      int lambda_index = 0;
      // calculate A^T * Y
      DenseColumnMatrix AtrY(N);
      DenseColumnMatrix measuredDataRef;
      if (sensorWeighting_)
      {
        measuredDataRef = CCtr * *measuredData_;
      }
      else
      {
        measuredDataRef = *measuredData_;
      }
      AtrY = forwardMatrix_->transpose() * measuredDataRef;
      DenseMatrix regForMatrix(N, N);
      DenseColumnMatrix solution(N);
      DenseColumnMatrix Ax(M);
      DenseColumnMatrix Rx(N);
      if ((input.regMethod_ == "single") || (input.regMethod_ == "slider"))
      {
        if (input.regMethod_ == "single")
        {
          // Use single fixed lambda value, entered in UI
          lambda = input.lambdaFromTextEntry_;
        }
        else if (input.regMethod_ == "slider")
        {
          // Use single fixed lambda value, select via slider
          lambda = input.lambdaSlider_;
        }
      }
      else if (input.regMethod_ == "lcurve")
      {
        // Use L-curve, lambda from corner of the L-curve
        const int nLambda = input.lambdaCount_;

        std::vector<double> lambdaArray(nLambda);
        std::vector<double> rho(nLambda);
        std::vector<double> eta(nLambda);

        lambdaArray[0] = input.lambdaMin_;
        const double lam_step = pow(10.0, log10(input.lambdaMax_ / input.lambdaMin_) / (nLambda-1));

        double* AtrAptr = AtrA.data();
        double* RtrRptr = RtrR.data();
        double* rm   = regForMatrix.data();

        int s = N*N;
        for (int j = 0; j < nLambda; j++)
        {
          if (j)
          {
            lambdaArray[j] = lambdaArray[j-1] * lam_step;
          }

          lambda_sq = lambdaArray[j] * lambdaArray[j];

          ///////////////////////////////////////
          ////Calculating the solution directly
          ///////////////////////////////////////
          for (int i = 0; i < s; i++)
          {
            rm[i] = AtrAptr[i] + lambda_sq * RtrRptr[i];
          }

          // Before, solution will be equal to (A^T * y)
          // After, solution will be equal to x_reg
          try
          {
            LinearAlgebra::solve_lapack(regForMatrix, AtrY, solution);
          }
          catch (LinearAlgebra::LapackError&)
          {
            const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda value for the L-Curve calculation.");
            if (pr_)
            {
              pr_->error(errorMessage);
            }
            else
            {
              std::cerr << errorMessage << std::endl;
            }
            throw;
          }
          catch(DimensionMismatch&)
          {
            const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
            if (pr_)
            {
              pr_->error(errorMessage);
            }
            else
            {
              std::cerr << errorMessage << std::endl;
            }
            throw;
          }

          ////////////////////////////////
          const DenseMatrix &forwardMatrixRef = *forwardMatrix_;

          Ax = forwardMatrixRef * solution;
          Rx = regMat * solution;
          // Calculate the norm of Ax-b and Rx
          rho[j]=0; eta[j]=0;
          for (int k = 0; k < M; k++)
          {
            double T = Ax(k) - (*measuredData_)(k);
            rho[j] += T*T;
          }

          for (int k = 0; k < N; k++)
          {
            double T = Rx(k);
            eta[j] += T*T;
          }

          rho[j] = sqrt(rho[j]);
          eta[j] = sqrt(eta[j]);
        }
        boost::shared_ptr<TikhonovAlgorithm::LCurveInput> lcurveInput(new TikhonovAlgorithm::LCurveInput(rho, eta, lambdaArray, nLambda));
        lcurveInput_handle_ = lcurveInput;
        lambda = FindCorner(*lcurveInput_handle_, lambda_index);

        if (input.updateLCurveGui_)
          input.updateLCurveGui_(lambda, *lcurveInput_handle_, lambda_index);

      } // END  else if (reg_method_.get() == "lcurve")
      lambda_sq = lambda * lambda;

      regularizationParameter_.reset(new DenseColumnMatrix(1));
      (*regularizationParameter_)[0] = lambda;
      int nr_rows = regForMatrix.nrows();
      int nr_cols = regForMatrix.ncols();
      const int NR_BOUNDS_CHECK = nr_rows * nr_cols;
      const double* AtrAptr = AtrA.data();
      const double* RtrRptr = RtrR.data();
      double* rm   = regForMatrix.data();

      for (int i = 0; i < NR_BOUNDS_CHECK; i++)
      {
        rm[i] = AtrAptr[i] + lambda_sq * RtrRptr[i];
      }
      if (computeRegularizedInverse_)
      {
        if (CCtr.empty())
          inverseMatrix_.reset(new DenseMatrix((regForMatrix.inverse() * forward_transpose)));
        else
          inverseMatrix_.reset(new DenseMatrix((regForMatrix.inverse() * forward_transpose) * CCtr));

        inverseSolution_.reset(new DenseMatrix(*inverseMatrix_ * *measuredData_));
      }
      else
      {
        try
        {
          LinearAlgebra::solve_lapack(regForMatrix, AtrY, solution);
        }
        catch (LinearAlgebra::LapackError&)
        {
          const std::string errorMessage("The Tikhonov linear system could not be solved for a regularization parameter in the Lambda Range of the L-curve. Use a higher Lambda value for the L-Curve calculation.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }
        catch(DimensionMismatch&)
        {
          const std::string errorMessage("Invalid matrix sizes are being used in the Tikhonov linear system.");
          if (pr_)
          {
            pr_->error(errorMessage);
          }
          else
          {
            std::cerr << errorMessage << std::endl;
          }
          throw;
        }
        inverseSolution_.reset(new DenseMatrix(solution));
      }

    }
}

void TikhonovAlgorithmImpl::update_graph(const TikhonovAlgorithmImpl::Input& input, double lambda, int lambda_index, const double epsilon)
{
  if (lcurveInput_handle_ && input.updateLCurveGui_)
  {
    lambda = LambdaLookup(*lcurveInput_handle_, lambda, lambda_index, epsilon);
    if (lambda >= 0)
    {
      input.updateLCurveGui_(lambda, *lcurveInput_handle_, lambda_index);
    }
  }
}

#if 0

void SolveInverseProblemWithTikhonov::tcl_command(GuiArgs& args, void* userdata)
{
  if (args[1] == "updategraph" && args.count() == 4)
  {
    double lambda = boost::lexical_cast<double>(args[2]);
    int lambda_index = boost::lexical_cast<double>(args[3]);

    if (input_handle_.get() != 0 && algo_handle_.get() != 0)
    {
      algo_handle_->update_graph(*input_handle_, lambda, lambda_index, lambda_resolution_.get());
    }
  }
  else
  {
    // Relay data to the Module class
    Module::tcl_command(args, userdata);
  }
}

#endif

TikhonovAlgorithm::LCurveInput::LCurveInput(const std::vector<double>& rho, const std::vector<double>& eta, const std::vector<double>& lambdaArray, int nLambda)
: rho_(rho), eta_(eta), lambdaArray_(lambdaArray), nLambda_(nLambda)
{}

TikhonovAlgorithmImpl::Input::Input(const std::string& regMethod, double lambdaFromTextEntry, double lambdaSlider, int lambdaCount, double lambdaMin, double lambdaMax,
                                    lcurveGuiUpdate updateLCurveGui)
: regMethod_(regMethod), lambdaFromTextEntry_(lambdaFromTextEntry), lambdaSlider_(lambdaSlider), lambdaCount_(lambdaCount), lambdaMin_(lambdaMin), lambdaMax_(lambdaMax),
updateLCurveGui_(updateLCurveGui)
{}

} // End namespace BioPSE