//==============================================================================
//         Copyright 2014          LRI    UMR 8623 CNRS/Univ Paris Sud XI
//         Copyright 2014          NumScale SAS
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_NUMERIC_ODEINT_EXTERNAL_NT2_NT2_NORM_INF_HPP_INCLUDED
#define BOOST_NUMERIC_ODEINT_EXTERNAL_NT2_NT2_NORM_INF_HPP_INCLUDED

#include <nt2/core/container/table/table.hpp>
#include <nt2/include/functions/globalmax.hpp>
#include <nt2/include/functions/abs.hpp>

#include <boost/numeric/odeint/algebra/vector_space_algebra.hpp>

namespace boost { namespace numeric { namespace odeint
{
  template<typename T, typename S>
  struct vector_space_norm_inf<nt2::container::table<T,S> >
  {
    typedef T result_type;
    result_type operator()(const nt2::container::table<T,S> &v1) const
    {
      return nt2::globalmax(nt2::abs(v1));
    }
  };
} } }

#endif


/*
Copyright 2012, 2013 Rogier van Dalen.

Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at

    http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
*/

/** \internal \file
Arithmetic operations on values stored as logarithms.
These also take Boost.Math policies with regard to error handling into account.
*/

#ifndef MATH_DETAIL_LOG_FLOAT_ARITHMETIC_HPP
#define MATH_DETAIL_LOG_FLOAT_ARITHMETIC_HPP

#include <cmath>
#include <cassert>
#include <limits>

#include <boost/static_assert.hpp>
#include <boost/math/policies/policy.hpp>
#include <boost/math/tools/promotion.hpp>
#include <boost/math/special_functions/fpclassify.hpp>

#include "./log-float_fwd.hpp"

namespace math { namespace detail {

    /*
    std::log1p gives exactly the behaviour we need: log1p (-1) should
    evaluate to -infinity, which indicates a stored value of 0.
    A policy could be used with boost::math::log1p, but this is slower.
    */
    using std::log1p;
    using std::exp;
    using std::log;

    /**
    Multiply two numbers represented as their logs.
    This is equivalent to adding them.
    However, this takes care to produce the correct errors where applicable.
    */
    template <class RealType1, class RealType2, class Policy,
            class OverflowPolicy, class IndeterminateResultPolicy>
        inline typename promote_args <RealType1, RealType2>::type
            multiply_log_float (RealType1 const & loga, RealType2 const & logb,
            Policy const & policy,
            OverflowPolicy const &, IndeterminateResultPolicy const &)
    {
        static char const * function_name = "multiplication of log_float<%1%>";
        typedef typename promote_args <RealType1, RealType2>::type
            result_type;
        static constexpr RealType1 a_infinity =
            std::numeric_limits <RealType1>::infinity();
        static constexpr RealType2 b_infinity =
            std::numeric_limits <RealType2>::infinity();
        static constexpr result_type result_infinity =
            std::numeric_limits <result_type>::infinity();
        static constexpr result_type result_nan =
            std::numeric_limits <result_type>::quiet_NaN();

        // This explicitly provides behaviour that IEEE standard floating
        // point numbers should provide.
        if (isnan (loga) || isnan (logb))
            return result_nan;
        else if (isinf (loga) || isinf (logb))
        {
            if (isinf (loga) && isinf (logb))
            {
                if (loga == a_infinity && logb == b_infinity) {
                    // +inf * +inf = +inf.
                    // Any overflow has happened before this operation.
                    return result_infinity;
                } else if (loga == -a_infinity && logb == -b_infinity) {
                    // 0 * 0 = 0.
                    // Any underflow has happened before this operation.
                    return - result_infinity;
                } else {
                    // loga and logb are of opposite signs.
                    return raise_indeterminate_result_error (
                        function_name, "%1% * inf is undefined",
                        result_type(), result_nan, Policy());
                }
            } else {
                // Either loga or logb is infinity,
                // either positive or negative.
                if (loga == -a_infinity || logb == -b_infinity)
                    // 0 * (finite value) = 0
                    return - result_infinity;
                else {
                    assert (loga == a_infinity || logb == b_infinity);
                    // +inf * (finite value) = +inf
                    return result_infinity;
                }
            }
        } else {
            result_type result = loga + logb;
            if (isinf (result)) {
                if (result == -result_infinity) {
                    raise_underflow_error <result_type> (function_name,
                        "Result of multiplication has underflowed",
                        Policy());
                } else {
                    assert (result == result_infinity);
                    raise_overflow_error <result_type> (function_name,
                        "Result of multiplication has overflowed",
                        Policy());
                }
            }
            return result;
        }
    }

    /**
    Multiply two numbers represented as their logs.
    This is equivalent to adding them.
    This implementation is used only when errors are ignored.
    It may be faster than the explicit implementation.
    */
    template <class RealType1, class RealType2, class Policy>
        inline typename promote_args <RealType1, RealType2>::type
            multiply_log_float (RealType1 const & loga, RealType2 const & logb,
                Policy const &,
                boost::math::policies::overflow_error<
                    boost::math::policies::ignore_error> const &,
                boost::math::policies::indeterminate_result_error<
                    boost::math::policies::ignore_error> const &)
    {
        return loga + logb;
    }

    /**
    Multiply two numbers represented as their logs.
    */
    template <class RealType1, class RealType2, class Policy>
        inline typename promote_args <RealType1, RealType2>::type
            multiply_log_float (RealType1 const & loga, RealType2 const & logb,
                Policy const & policy)
    {
        // Forward to an appropriate implementation depending on the
        // error policy.
        return multiply_log_float (loga, logb, policy,
            typename Policy::overflow_error_type(),
            typename Policy::indeterminate_result_error_type());
    }

    /**
    Divide two numbers represented as their logs.
    This is equivalent to subtraction.
    However, this takes care to produce the correct errors where applicable.
    */
    template <class NumeratorType, class DenominatorType, class Policy,
            class OverflowPolicy, class UnderflowPolicy,
            class IndeterminateResultPolicy>
        inline typename promote_args <NumeratorType, DenominatorType>::type
            divide_log_float (NumeratorType const & log_numerator,
                DenominatorType const & log_denominator,
                Policy const & policy,
                OverflowPolicy const &, UnderflowPolicy const &,
                IndeterminateResultPolicy const &)
    {
        static char const * function_name = "division of log_float<%1%>";
        typedef typename promote_args <NumeratorType, DenominatorType>::type
            result_type;
        static constexpr NumeratorType numerator_infinity =
            std::numeric_limits <NumeratorType>::infinity();
        static constexpr DenominatorType denominator_infinity =
            std::numeric_limits <DenominatorType>::infinity();
        static constexpr result_type result_infinity =
            std::numeric_limits <result_type>::infinity();
        static constexpr result_type result_nan =
            std::numeric_limits <result_type>::quiet_NaN();

        // This explicitly provides behaviour that IEEE standard floating
        // point numbers should provide.
        if (isnan (log_numerator) || isnan (log_denominator))
            return result_nan;
        else if (isinf (log_numerator) || isinf (log_denominator)) {
            if (log_denominator == -denominator_infinity) {
                if (log_numerator == -numerator_infinity) {
                    return raise_indeterminate_result_error (
                        function_name, "%1% / 0 is undefined",
                        result_type(), result_nan, Policy());
                } else {
                    if (log_numerator == numerator_infinity)
                        // inf / 0 = inf: not really overflow,
                        // just propagation of infinity.
                        return result_infinity;
                    else
                        return raise_overflow_error <result_type> (
                            function_name, "division by zero causes overflow",
                            Policy());
                }
            } else if (log_denominator == denominator_infinity) {
                if (log_numerator == numerator_infinity) {
                    return raise_indeterminate_result_error (
                        function_name, "%1% / %1% is undefined",
                        result_type (log_numerator), result_nan, Policy());
                } else {
                    // (finite value) / inf = 0.
                    // The overflow has already happened, so this is not
                    // classified as underflow.
                    return -result_infinity;
                }
            } else {
                assert (!isinf (log_denominator));
                assert (isinf (log_numerator));
                if (log_numerator == -numerator_infinity)
                    return -result_infinity;
                else {
                    assert (log_numerator == numerator_infinity);
                    return result_infinity;
                }
            }
        } else {
            result_type result = log_numerator - log_denominator;
            if (isinf (result)) {
                if (result == -result_infinity
                        && log_numerator != -numerator_infinity) {
                    raise_underflow_error <result_type> (function_name,
                        "Result of division has underflowed",
                        Policy());
                } else {
                    assert (result == result_infinity);
                    if (log_numerator != numerator_infinity)
                        raise_overflow_error <result_type> (function_name,
                            "Result of division has overflowed",
                            Policy());
                }
            }
            return result;
        }
    }

    /**
    Divide two numbers represented as their logs.
    This is equivalent to subtraction.
    This implementation is used only when errors are ignored.
    It may be faster than the explicity implementation.
    */
    template <class NumeratorType, class DenominatorType, class Policy>
        inline typename promote_args <NumeratorType, DenominatorType>::type
            divide_log_float (NumeratorType const & log_numerator,
                DenominatorType const & log_denominator,
                Policy const &,
                boost::math::policies::overflow_error<
                    boost::math::policies::ignore_error> const &,
                boost::math::policies::underflow_error<
                    boost::math::policies::ignore_error> const &,
                boost::math::policies::indeterminate_result_error<
                    boost::math::policies::ignore_error> const &)
    { return log_numerator - log_denominator; }

    /**
    Divide two numbers represented as their logs.
    This is equivalent to subtraction.
    */
    template <class NumeratorType, class DenominatorType, class Policy>
        inline typename promote_args <NumeratorType, DenominatorType>::type
            divide_log_float (NumeratorType const & log_numerator,
                DenominatorType const & log_denominator,
                Policy const & policy)
    {
        // Forward to an appropriate implementation depending on the
        // error policy.
        return divide_log_float (log_numerator, log_denominator, policy,
            typename Policy::overflow_error_type(),
            typename Policy::underflow_error_type(),
            typename Policy::indeterminate_result_error_type());
    }

    /**
    Add two numbers represented as their logs.
    This takes care to produce the correct errors where applicable.
    */
    template <class RealType1, class RealType2,
            class Policy, class OverflowPolicy>
        inline typename promote_args <RealType1, RealType2>::type
            add_log_float (RealType1 const & loga, RealType2 const & logb,
            Policy const & policy, OverflowPolicy const &)
    {
        static char const * function_name = "addition of log_float<%1%>";
        typedef typename promote_args <RealType1, RealType2>::type
            result_type;
        static constexpr RealType1 a_infinity =
            std::numeric_limits <RealType1>::infinity();
        static constexpr RealType2 b_infinity =
            std::numeric_limits <RealType2>::infinity();
        static constexpr result_type result_infinity =
            std::numeric_limits <result_type>::infinity();
        static constexpr result_type result_nan =
            std::numeric_limits <result_type>::quiet_NaN();

        // This explicitly provides behaviour that IEEE standard floating
        // point numbers should provide.
        if (isnan (loga) || isnan (logb))
            return result_nan;
        else if (isinf (loga) && isinf (logb)) {
            if (loga == -a_infinity && logb == -b_infinity)
                return -result_infinity;
            else {
                assert (loga == a_infinity || logb == b_infinity);
                // 0 + inf = inf
                // inf + inf = inf
                return result_infinity;
            }
        } else {
            result_type result;
            if (loga > logb)
                result = loga + log1p (exp (logb - loga));
            else
                result = logb + log1p (exp (loga - logb));

            if (result == result_infinity &&
                    loga != a_infinity && logb != b_infinity) {
                raise_overflow_error <result_type> (function_name,
                    "Result of addition has overflowed",
                    Policy());
            }
            return result;
        }
    }

    /**
    Add two numbers represented as their logs.
    This implementation, for policies that ignore errors, is somewhat
    faster.
    */
    template <class RealType1, class RealType2, class Policy>
        inline typename promote_args <RealType1, RealType2>::type
            add_log_float (RealType1 const & loga, RealType2 const & logb,
                Policy const & policy,
                boost::math::policies::overflow_error <
                    boost::math::policies::ignore_error> const &)
    {
        typedef typename promote_args <RealType1, RealType2>::type
            result_type;

        /*
        A normal implementation of addition in the log-domain uses,
        (assuming, without loss of generalisation, that loga >= logb):
        1)  log (a + b) = loga + log (1 + exp (logb - loga))
            where implementing log (1 + ...) with log1p yields marginal
            improvements.
        Corner cases are NaNs and infinities.
        The following implementation aims to use as few cases as possible
        by allowing most corner cases to be dealt with by the IEEE floating
        point operations:
        2)  isnan (loga) || isnan (logb):
            Any computation that involves both loga and logb, such as the
            one for case (1), returns NaN, which is correct.
        Infinities (assume again that loga >= logb):
        3)  If (loga == logb), either both +infinity, or both -infinity:
            logb - loga = NaN
            and the result will be NaN, which is incorrect.
            This requires a special case.
            A solution is to return loga or logb.
        4)  Otherwise, (loga == +infinity || logb == -infinity):
            The default implementation returns loga:
                logb - loga = -infinity
                exp (logb - loga) = 0
                log1p (0) = 0
                loga + 0 = loga
            which is correct.
            No special handling is required.
        Note, however, that for loga >= logb, (3) and (4) are handled by
            return loga
        and vice versa.
        */
        using std::abs;
        using std::max;

        if (!::boost::math::isinf (loga)) {
            result_type difference = -abs (logb - loga);
            result_type greatest = (max) (loga, logb);
            return greatest + log1p (exp (difference));
        } else {
            if (!(loga >= logb))
                return result_type (logb);
            else
                return result_type (loga);
        }
    }

    /**
    Add two numbers represented as their logs.
    */
    template <class RealType1, class RealType2, class Policy>
        inline typename promote_args <RealType1, RealType2>::type
            add_log_float (RealType1 const & loga, RealType2 const & logb,
            Policy const & policy)
    {
        return add_log_float (loga, logb, policy,
            typename Policy::overflow_error_type());
    }

    /**
    \pre !(loga < logb)
        (This is not the same as loga >= logb in the face of NaNs.)
    */
    template <class RealType1, class RealType2, class Policy,
            class UnderflowPolicy, class IndeterminateResultPolicy>
        inline typename promote_args <RealType1, RealType2>::type
            subtract_log_float (RealType1 const & loga, RealType2 const & logb,
            Policy const & policy,
            UnderflowPolicy const &, IndeterminateResultPolicy const &)
    {
        assert (!(loga < logb));
        static char const * function_name = "subtraction of log_float<%1%>";
        typedef typename promote_args <RealType1, RealType2>::type
            result_type;
        static constexpr RealType1 a_infinity =
            std::numeric_limits <RealType1>::infinity();
        static constexpr RealType2 b_infinity =
            std::numeric_limits <RealType2>::infinity();
        static constexpr result_type result_infinity =
            std::numeric_limits <result_type>::infinity();
        static constexpr result_type result_nan =
            std::numeric_limits <result_type>::quiet_NaN();

        // This explicitly provides behaviour that IEEE standard floating
        // point numbers should provide.
        if (isnan (loga) || isnan (logb))
            return result_nan;
        else if (loga == a_infinity) {
            if (logb == b_infinity) {
                return raise_indeterminate_result_error (
                    function_name, "%1% - inf is undefined",
                    result_infinity, result_nan, policy);
            } else
                // Any overflow has taken place already.
                return result_infinity;
        } else if (loga == logb) {
            return -result_infinity;
        } else {
            result_type result;
            result = loga + log1p (-exp (logb - loga));

            if (result == - result_infinity) {
                raise_underflow_error <result_type> (function_name,
                    "Result of subtraction has underflowed",
                    Policy());
            }
            return result;
        }
    }

    template <class RealType1, class RealType2, class Policy>
        inline typename promote_args <RealType1, RealType2>::type
            subtract_log_float (RealType1 const & loga, RealType2 const & logb,
            Policy const & policy,
                boost::math::policies::underflow_error<
                    boost::math::policies::ignore_error> const &,
                boost::math::policies::indeterminate_result_error<
                    boost::math::policies::ignore_error> const &)
    {
        assert (!(loga < logb));
        if (loga == -std::numeric_limits <RealType1>::infinity())
            // logb == -infinity or nan.
            return loga + logb;
        else
            return loga + log1p (-exp (logb - loga));
    }

    /**
    \pre !(loga < logb)
        (This is not the same as loga >= logb in the face of NaNs.)
    */
    template <class RealType1, class RealType2, class Policy>
        inline typename promote_args <RealType1, RealType2>::type
            subtract_log_float (RealType1 const & loga, RealType2 const & logb,
            Policy const & policy)
    {
        return subtract_log_float (loga, logb, policy,
            typename Policy::underflow_error_type(),
            typename Policy::indeterminate_result_error_type());
    }

    template <class RealType1, class RealType2, class Policy>
        inline std::pair <
            typename promote_args <RealType1, RealType2>::type, int>
        add_signed_log_float (RealType1 const & loga, int sign_a,
            RealType2 const & logb, int sign_b,
            Policy const & policy)
    {
        if (sign_a == sign_b)
            return std::make_pair (add_log_float (loga, logb, policy), sign_a);
        else {
            if (loga >= logb)
                return std::make_pair (
                    subtract_log_float (loga, logb, policy), sign_a);
            else
                return std::make_pair (
                    subtract_log_float (logb, loga, policy), sign_b);
        }
    }

}} // namespace math::detail

#endif  // MATH_DETAIL_LOG_FLOAT_ARITHMETIC_HPP


//
// Created by asem on 01/01/19.
//

#ifndef MARKOVIAN_FEATURES_AAINDEXCLUSTERING_HPP
#define MARKOVIAN_FEATURES_AAINDEXCLUSTERING_HPP

#include "dlib_utilities.hpp"
#include <dlib/svm.h>

#include "SimilarityMetrics.hpp"
#include "LabeledEntry.hpp"
#include "AAIndexDBGET.hpp"
#include "common.hpp"
#include "LUT.hpp"

namespace aaindex {

class AAIndexClustering
{
    using SampleType = dlib::matrix<double, 0, 0>;

public:
    explicit AAIndexClustering(
            const std::vector<AAIndex1> &index,
            size_t nClusters
    )
            : _index( index ),
              _dims( index.size()),
              _nClusters( nClusters )
    {}

    virtual void runClustering()
    {

        auto samplesVector = _samples();

        {
            std::vector<SampleType> centroids;

            dlib::pick_initial_centers( _nClusters, centroids, samplesVector );
            dlib::find_clusters_using_kmeans( samplesVector, centroids );

            _centroids.emplace( std::move( centroids ));
        }

        _clusters.emplace( LUT<char, long>::makeLUT(
                [this]( char aa ) -> long {
                    if ( auto point = getPoint( aa ); point )
                        return _closestIdx( _centroids.value(), point.value());
                    else return -1;
                } ));
    }

    std::optional<SampleType> getPoint( char aa ) const
    {
        using namespace dlib_utilities;
        std::vector<double> point;
        for (auto &index : _index)
        {
            if ( auto component = index.normalizedIndex( aa ); component )
                point.push_back( component.value());
            else return std::nullopt;
        }
        return vector_to_column_matrix_like( std::move( point ));
    }

    std::optional<size_t> getCluster( char aa ) const
    {
        assert( _centroids.has_value() && _clusters.has_value());
        if ( auto cluster = _clusters->at( aa ); cluster >= 0 )
            return size_t( cluster );
        else return std::nullopt;
    }

protected:

    long _closestIdx(
            const std::vector<SampleType> &centroids,
            const SampleType &point
    )
    {
        static auto euclidean = Euclidean::template similarityFunctor<SampleType>();

        auto minIt = std::min_element(
                centroids.cbegin(), centroids.cend(),
                [&](
                        SampleType x,
                        SampleType y
                ) {
                    return euclidean( x, point ) < euclidean( y, point );
                } );
        return std::distance( centroids.cbegin(), minIt );
    }

    std::vector<SampleType> _samples() const
    {
        std::vector<SampleType> samples;
        for (auto aa : AMINO_ACIDS20)
        {
            auto point = getPoint( aa );
            assert( point.has_value());
            samples.emplace_back( getPoint( aa ).value());
        }
        return samples;
    }

private:
    const std::vector<AAIndex1> _index;
    const size_t _nClusters;
    const size_t _dims;

    std::optional<std::vector<SampleType>> _centroids;
    std::optional<LUT<char, long>> _clusters;
};

}

#endif //MARKOVIAN_FEATURES_AAINDEXCLUSTERING_HPP


/*
********************************************************************************
MIT License

Copyright(c) 2018 Christopher Brandt

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.
********************************************************************************
*/
#include <iostream>
#include <math.h>

#include "ProjDynRHSInterpol.h"
#include "ProjDynConstraints.h"
#include "ProjDynSimulator.h"
#include "ProjUtils/ProjDynUtil.h"
#include <Eigen/Eigenvalues>

PDMatrix PD::RHSInterpolationGroup::snapshotPCA(PDMatrix& Y, PDVector& masses, unsigned int size)
{
	/* PCA via the method of snapshots*/

	std::cout << "Performing snapshot PCA..." << std::endl;

	PDMatrix base(Y.rows(), size + 1);

	std::cout << "	Removing column wise mean..." << std::endl;

	// Remove column wise mean
	PROJ_DYN_PARALLEL_FOR
		for (int j = 0; j < Y.cols(); j++) {
			PDScalar mean = Y.col(j).mean();
			for (unsigned int i = 0; i < Y.rows(); i++) {
				Y(i, j) -= mean;
			}
		}

	// We solve the SVD by solving an eigenvalue problem on the smaller matrix A = Y^T M Y
	// and converting the eigenvectors to that problem back to v = Y u
	std::cout << "	Computing Y^T M Y..." << std::endl;
	PDMatrix A;
#ifdef PROJ_DYN_USE_CUBLAS_IN_PRE
	std::cout << "		uploading Y to GPU..." << std::endl;
	CUDAMatrixVectorMultiplier* yMulti = new CUDAMatrixVectorMultiplier(Y, masses);
	std::cout << "		computing Y^T (M Y) on GPU..." << std::endl;
	A.resize(Y.cols(), Y.cols());
	for (unsigned int i = 0; i < Y.cols(); i++) {
		PDScalar one = 1;
		yMulti->mult(Y.data() + (i * Y.rows()), A.data() + (i * A.rows()), one, true);
	}
#else
	PDMatrix Y_T = Y.transpose();
	PROJ_DYN_PARALLEL_FOR
		for (int j = 0; j < Y.cols(); j++) {
			Y_T.col(j) *= masses(j);
		}
	A.resize(Y.cols(), Y.cols());
	PROJ_DYN_PARALLEL_FOR
		for (int j = 0; j < Y.cols(); j++) {
			A.col(j) = Y_T * Y.col(j);
		}
#endif
	std::cout << "	Computing eigenvectors v of Y^T M Y..." << std::endl;
	Eigen::EigenSolver< PDMatrix > eSolver;
	eSolver.compute(A, true);
	Eigen::Matrix< std::complex< double >, -1, -1> eVecsC = eSolver.eigenvectors();
	Eigen::Matrix< std::complex< double >, -1, 1> eValsC = eSolver.eigenvalues();
	std::cout << "	Extracting PCA vectors as Y v..." << std::endl;
	PDScalar currentEVal = -1;
	std::vector<unsigned int> eVecInds;
	for (int v = 0; v < size; v++) {
		PDScalar currentLargest = 0;
		int indOfLargest = -1;
		for (int j = 0; j < eValsC.rows(); j++) {
			if (eValsC(j).real() > currentLargest && (currentEVal < 0 || eValsC(j).real() < currentEVal)) {
				currentLargest = eValsC(j).real();
				indOfLargest = v;
			}
		}
		if (indOfLargest >= 0) {
			eVecInds.push_back(indOfLargest);
			currentEVal = eValsC(indOfLargest).real();
			//std::cout << eValsC(indOfLargest).real() << std::endl;
		}
		else {
			std::cout << "Could only find " << (v - 1) << " eigenvectors with strictly positive eigenvalues during snapshot PCA ..." << std::endl;
			base.conservativeResize(base.rows(), std::max(1, v - 10));
			break;
		}
	}

#ifdef PROJ_DYN_USE_CUBLAS_IN_PRE
	for (int v = 0; v < std::min((int)base.cols(), (int)eVecInds.size()); v++) {
		PDScalar weight = (1. / std::sqrt(eValsC(eVecInds[v]).real()));
		PDVector eVec = eVecsC.col(eVecInds[v]).real();
		yMulti->mult(eVec.data(), base.data() + (v * base.rows()), weight, false);
	}
	delete yMulti;
#else
	PROJ_DYN_PARALLEL_FOR
		for (int v = 0; v < std::min((int)base.cols(), (int)eVecInds.size()); v++) {
			PDVector eVec = eVecsC.col(eVecInds[v]).real();
			base.col(v) = Y * eVec;
			PDScalar weight = (1. / std::sqrt(eValsC(eVecInds[v]).real()));
			base.col(v) *= weight;
		}
#endif

	if (base.hasNaN()) {
		std::cout << "Warning: base contains NaN values!" << std::endl;
	}

	// Make sure to include global translations since we removed column wise mean
	base.col(base.cols() - 1).setConstant(1.);

	std::cout << "	Done." << std::endl;

	return base;
}

PD::RHSInterpolationGroup::RHSInterpolationGroup(std::string groupName, std::vector<ProjDynConstraint*>& constraints, PDPositions& restPositions,
	PDVector& vertexMasses, PDTriangles& tris, PDTets& tets, PDScalar regularizationWeight)
	:
	m_auxiliarySize(0),
	m_basis(0, 0),
	m_interpolReady(false),
	m_basisReady(false),
	m_groupName(groupName),
	m_regularizationWeight(regularizationWeight),
	m_restPositionFull(restPositions)
{
	for (auto c : constraints) {
		//ProjDynConstraint* cc = c->copy();
		m_constraints.push_back(c);
	}

	m_isTetExampleGroup = (m_groupName == "tetex");

	if (m_constraints.size() >= 1) {
		int dummy = -1;
		m_auxiliarySize = m_constraints.at(0)->getP(restPositions, dummy).rows();

		// Compute the mass matrix for the unassembled auxiliaries
		computeMassMatrix(vertexMasses, tris, tets);
	}
}

PD::RHSInterpolationGroup::RHSInterpolationGroup(std::string groupName, unsigned int auxiliarySize)
	:
	m_groupName(groupName),
	m_auxiliarySize(auxiliarySize),
	m_basis(0, 0),
	m_selectionMatrix(0, 0),
	m_interpolReady(false),
	m_basisReady(false),
	m_regularizationWeight(0.),
	m_restPositionFull(0, 3)
{
}

void PD::RHSInterpolationGroup::computeMassMatrix(PDVector& vertexMasses, PDTriangles& tris, PDTets& tets) {
	// Compute the mass matrix for the unassembled auxiliaries
	m_massMatrix.resize(m_constraints.size() * m_auxiliarySize, m_constraints.size() * m_auxiliarySize);
	m_massMatrixDiag.resize(m_constraints.size() * m_auxiliarySize);
	int i = 0;
	for (auto& c : m_constraints) {
		double weight = 1.;
		if (!NO_MASSES_IN_PCA) {
			if (c->getMainVertexIndex() > 0) {
				weight = vertexMasses(c->getMainVertexIndex());
			}
			else if (c->getMainTriangleIndex() > 0) {
				weight = 0;
				for (int j = 0; j < 3; j++) {
					weight += vertexMasses(tris(c->getMainTriangleIndex(), j));
				}
			}
			else if (c->getMainTetIndex() > 0) {
				weight = 0;
				for (int j = 0; j < 4; j++) {
					weight += vertexMasses(tets(c->getMainTetIndex(), j));
				}
			}
		}
		for (int j = 0; j < m_auxiliarySize; j++) {
			m_massMatrix.insert(i * m_auxiliarySize + j, i * m_auxiliarySize + j) = weight;
			m_massMatrixDiag(i * m_auxiliarySize + j) = weight;
		}
		i++;
	}
}

void PD::RHSInterpolationGroup::createBasisViaSkinningWeights(unsigned int size, PDPositions& restPos, PDMatrix& skinningWeights, bool usePCA,
	PDTriangles& tris, PDTets &tets)
{
	std::cout << "Creating interpolation basis via skinning construction for " << getName() << "... " << std::endl;
	std::cout << "	Computing rest state auxiliary variables..." << std::endl;
	PDPositions restStateAuxils = PD::getAssemblyMatrix(m_constraints, true, NO_WEIGHTS_IN_CONSTRUCTION).transpose() * restPos;
	std::cout << "	Computing skinning space..." << std::endl;
	PDMatrix Y = PD::createSkinningSpace(restStateAuxils, skinningWeights, &m_constraints, m_auxiliarySize, &tris, &tets);

	if (!usePCA) {
		m_basis = Y;
	}
	else {
		m_basis = RHSInterpolationGroup::snapshotPCA(Y, m_massMatrixDiag, size);
	}
	m_basisReady = true;
	m_interpolReady = false;
}

void PD::RHSInterpolationGroup::setBasis(PDMatrix & base)
{
	m_basis = base;

	m_basisReady = true;
	m_interpolReady = false;

}

void PD::RHSInterpolationGroup::initInterpolation(unsigned int numVertices, std::vector<unsigned int>& sampledElements,
	PDSparseMatrix const& positionSubspaceT)
{
	std::cout << "Finalizing rhs interpolation for " << getName() << "... " << std::endl;
	std::cout << "	Computing assembly, selection and weight matrix..." << std::endl;

	std::vector<unsigned int> sampledElsCopy = sampledElements;

	// Create assembly matrix
	m_assemblyMatrix = PD::getAssemblyMatrix(m_constraints, true, NO_WEIGHTS_IN_CONSTRUCTION);

	// Create selection and weights matrix
	std::vector<Eigen::Triplet<PDScalar>> selectedInds;
	m_weightMatrix.resize(m_constraints.size() * m_auxiliarySize, m_constraints.size() * m_auxiliarySize);
	PDVector weightVector(m_constraints.size() * m_auxiliarySize);
	weightVector.setConstant(1.);
	for (unsigned int i = 0; i < m_constraints.size(); i++) {
		ProjDynConstraint* c = m_constraints[i];
		unsigned int constraintElement = -1;
		if (c->getMainVertexIndex() >= 0) {
			constraintElement = c->getMainVertexIndex();
		}
		else if (c->getMainTriangleIndex() >= 0) {
			constraintElement = c->getMainTriangleIndex();
		}
		else if (c->getMainTetIndex() >= 0) {
			constraintElement = c->getMainTetIndex();
		}
		auto const& it = std::find(sampledElsCopy.begin(), sampledElsCopy.end(), constraintElement);
		if (m_sampledConstraints.size() < sampledElements.size() && it != sampledElsCopy.end()) {
			sampledElsCopy.erase(it);
			m_sampledConstraints.push_back(c->copy());
			int ind = m_sampledConstraints.size() - 1;
			for (int d = 0; d < m_auxiliarySize; d++) {
				selectedInds.push_back(Eigen::Triplet<PDScalar>(ind * m_auxiliarySize + d, i * m_auxiliarySize + d, 1.));
			}
		}
		for (int j = 0; j < m_auxiliarySize; j++) {
			if (NO_WEIGHTS_IN_CONSTRUCTION) {
				m_weightMatrix.insert(i * m_auxiliarySize + j, i * m_auxiliarySize + j) = c->getWeight();
				weightVector(i * m_auxiliarySize + j) = c->getWeight();
			}
			else {
				// Weights are included in the interpolation problem and snapshot basis
				m_weightMatrix.insert(i * m_auxiliarySize + j, i * m_auxiliarySize + j) = 1.;
			}
		}
	}
	m_selectionMatrix = PDSparseMatrix(sampledElements.size() * m_auxiliarySize, m_constraints.size() * m_auxiliarySize);
	m_selectionMatrix.setZero();
	m_selectionMatrix.setFromTriplets(selectedInds.begin(), selectedInds.end());
	// If we couldn't find as many constraint samples as we have sampled
	// elements, we overestimated the number of rows in the selection matrix
	// and need to cut off the rest.
	if (m_sampledConstraints.size() < sampledElements.size()) {
		m_selectionMatrix.conservativeResize(m_sampledConstraints.size() * m_auxiliarySize, m_selectionMatrix.cols());
	}
	if (m_selectionMatrix.rows() < m_basis.cols() - MIN_OVERSAMPLING) {
		std::cout << "Warning: not enough sampled constraints (" << m_sampledConstraints.size()
			<< ") for this rhs interpolation base (" << m_basis.cols() << ")." << std::endl;
		std::cout << "	Cutting off last vectors in base." << std::endl;
		m_basis.conservativeResize(m_basis.rows(), m_selectionMatrix.rows() - MIN_OVERSAMPLING);
	}

	// Compute lhs and rhs matrices and solver for the inteprolation problem
	std::cout << "	Computing lhs and rhs matrices for linear interpolation system and factorizing..." << std::endl;
	m_interpolRHSMatrix = m_basis.transpose() * m_selectionMatrix.transpose();
	PDMatrix lhsMat;
	if (m_regularizationWeight > 0) {
		lhsMat = m_basis.transpose() * ((m_selectionMatrix.transpose() * m_selectionMatrix) + m_regularizationWeight * m_massMatrix) * m_basis;
	}
	else {
		lhsMat = m_basis.transpose() * ((m_selectionMatrix.transpose() * m_selectionMatrix)) * m_basis;
	}
	m_interpolSolver.compute(lhsMat);
	m_tempSolution.resize(lhsMat.rows(), 3);
	m_solutionLastFrame.resize(lhsMat.rows(), 3);
	m_tempAuxils.resize(m_selectionMatrix.rows(), 3);
	m_tempRHS.resize(lhsMat.rows(), 3);

	// If regularization is used we have to get some initial value for m_solutionLastFrame
	// For this we have to evaluate the auxilaries for the rest state and set up a projection
	// problem, which returns the best subspace approximation of these auxilaries in
	// the interpolation base.
	if (m_regularizationWeight > 0) {
		std::cout << "	Computing extra stuff for regularization..." << std::endl;
		m_regularizationRHSMatrix = m_basis.transpose() * m_massMatrix * m_basis;
		PDPositions rhsFull(m_constraints.size() * m_auxiliarySize, 3);
		// Collect auxiliary variables for sampled constraints
		int didCollide = -1;
		PROJ_DYN_PARALLEL_FOR
			for (int i = 0; i < m_constraints.size(); i++) {
				PDScalar weight = 1.;
				if (!NO_WEIGHTS_IN_CONSTRUCTION) {
					weight = std::sqrt(m_constraints[i]->getWeight());
				}
				didCollide = -1;
				rhsFull.block(i*m_auxiliarySize, 0, m_auxiliarySize, 3) = m_constraints[i]->getP(m_restPositionFull, didCollide)
					* weight;
			}
		PDPositions rhsSub = m_basis.transpose() * m_massMatrix * rhsFull;
		PDMatrix projLHS = m_basis.transpose() * m_massMatrix * m_basis;
		Eigen::LLT<PDMatrix> projSolver;
		projSolver.compute(projLHS);
		for (unsigned int d = 0; d < 3; d++) {
			m_solutionLastFrame.col(d) = projSolver.solve(rhsSub.col(d));
		}
	}

	// Compute the finalization matrix, i.e. the one that maps a vector from the subspace
	// spanned by the snapshots' PCA to a fully assembled rhs.
	// In case a positions subspace is used, the transpose of the subspace matrix will
	// also be applied to the matrix
	std::cout << "	Computing 'finalize' matrices..." << std::endl;
	if (positionSubspaceT.rows() > 0) {
		std::cout << "		sparse part..." << std::endl;
		PDSparseMatrix finalTemp = positionSubspaceT * m_assemblyMatrix;
		if (NO_WEIGHTS_IN_CONSTRUCTION) {
			PROJ_DYN_PARALLEL_FOR
				for (int c = 0; c < finalTemp.cols(); c++) {
					finalTemp.col(c) *= weightVector(c);
				}
		}
		std::cout << "		dense part..." << std::endl;
		m_finalizeMatrix.resize(positionSubspaceT.rows(), m_basis.cols());
		PROJ_DYN_PARALLEL_FOR
			for (int c = 0; c < m_basis.cols(); c++) {
				m_finalizeMatrix.col(c) = finalTemp * m_basis.col(c);
			}
		m_usingPositionSubspace = true;
	}
	else {
		m_finalizeMatrixBig = m_assemblyMatrix * m_weightMatrix * m_basis;
		m_usingPositionSubspace = false;
	}

	// Finally compute the solver for the projection problem if desired
	if (PROJ_DYN_ENABLE_RHS_PROJECTION) {
		std::cout << "	Computing projection solver for this group..." << std::endl;
		m_projectionSolver.compute(m_basis.transpose() * m_basis);
		if (m_usingPositionSubspace) {
			m_projectionFinalizeMatrix = positionSubspaceT * m_assemblyMatrix;
		}
	}

	std::cout << "Initiated snapshot interpolation for group " << m_groupName << "." << std::endl;

	m_interpolReady = true;
}

void PD::RHSInterpolationGroup::approximateRHS(PDPositions & pos, PDPositions & rhs, bool * collidedVertices)
{
	if (!m_interpolReady) {
		std::cout << "Interpolation is not initialized!!!" << std::endl;
		return;
	}

	interpolate(pos, m_tempSolution, collidedVertices);

	// Apply finalization matrix and add to current rhs
	if (m_usingPositionSubspace) {
		PROJ_DYN_PARALLEL_FOR
			for (int d = 0; d < 3; d++) {
				rhs.col(d) += m_weightFac * m_finalizeMatrix * m_tempSolution.col(d);
				//std::cout << m_finalizeMatrix * m_tempSolution.col(d) << std::endl;
			}
	}
	else {
		PROJ_DYN_PARALLEL_FOR
			for (int d = 0; d < 3; d++) {
				rhs.col(d) += m_weightFac * m_finalizeMatrixBig * m_tempSolution.col(d);
			}
	}
}

void PD::RHSInterpolationGroup::evaluateRHS(PDPositions & pos, PDPositions & rhs, bool projectToInterpolationSpaceAndBack, bool forceFull)
{
	if (PROJ_DYN_ENABLE_RHS_PROJECTION) {
		PDPositions auxils(m_constraints.size() * m_auxiliarySize, 3);
		PDPositions auxilsNoWeights(m_constraints.size() * m_auxiliarySize, 3);
		// Collect auxiliary variables for sampled constraints
		int didCollide = -1;
		PROJ_DYN_PARALLEL_FOR
			for (int i = 0; i < m_constraints.size(); i++) {
				didCollide = -1;
				PDScalar weight = m_constraints[i]->getWeight();
				if (!NO_WEIGHTS_IN_CONSTRUCTION) {
					weight = std::sqrt(weight);
				}
				auxils.block(i*m_auxiliarySize, 0, m_auxiliarySize, 3) = m_constraints[i]->getP(pos, didCollide)
					* weight * m_weightFac;
				auxilsNoWeights.block(i*m_auxiliarySize, 0, m_auxiliarySize, 3) = m_constraints[i]->getP(pos, didCollide);
			}

		if (projectToInterpolationSpaceAndBack) {
			PDPositions solutionSub = m_projectionSolver.solve(m_basis.transpose() * auxilsNoWeights);
			auxils = m_basis * solutionSub;
			PROJ_DYN_PARALLEL_FOR
				for (int i = 0; i < m_constraints.size(); i++) {
					didCollide = -1;
					PDScalar weight = m_constraints[i]->getWeight();
					if (!NO_WEIGHTS_IN_CONSTRUCTION) {
						weight = std::sqrt(weight);
					}
					auxils.block(i*m_auxiliarySize, 0, m_auxiliarySize, 3) *= weight * m_weightFac;
				}
		}

		if (m_usingPositionSubspace && !forceFull) {
			rhs += m_projectionFinalizeMatrix * auxils;
		}
		else {
			rhs += m_assemblyMatrix * auxils;
		}
	}
	else {
		std::cout << "Enable PROJ_DYN_ENABLE_RHS_PROJECTION if you desire to evaluate full rhs!" << std::endl;
	}
}

void PD::RHSInterpolationGroup::interpolate(PDPositions & pos, PDPositions & interpol, bool* collidedVertices)
{
	// Collect auxiliary variables for sampled constraints
	int didCollide = -1;
	PROJ_DYN_PARALLEL_FOR
		for (int i = 0; i < m_sampledConstraints.size(); i++) {
			didCollide = -1;
			PDScalar weight = 1.;
			if (!NO_WEIGHTS_IN_CONSTRUCTION) {
				weight = std::sqrt(m_sampledConstraints[i]->getWeight());
			}
			m_tempAuxils.block(i*m_auxiliarySize, 0, m_auxiliarySize, 3) = m_sampledConstraints[i]->getP(pos, didCollide)
				* weight * m_blowUp;
			if (collidedVertices && didCollide > 0) {
				collidedVertices[didCollide] = true;
			}
		}

	// Set up rhs from auxiliaries and solve the system for all three columns
	PROJ_DYN_PARALLEL_FOR
		for (int d = 0; d < 3; d++) {
			m_tempRHS.col(d) = m_interpolRHSMatrix * m_tempAuxils.col(d);
			if (m_regularizationWeight > 0.) {
				m_tempRHS.col(d) += m_regularizationWeight * m_regularizationRHSMatrix * m_solutionLastFrame.col(d);
			}
			interpol.col(d) = m_interpolSolver.solve(m_tempRHS.col(d));
		}

	m_solutionLastFrame = interpol;
}

PDMatrix PD::RHSInterpolationGroup::getLHSMatrixSubspace(PDMatrix& posSubspaceMat, PDMatrix& posSubspaceMat_T)
{
	if (!m_lhsMatrixSubspaceInitialized) {
		PDSparseMatrix temp;
		if (NO_WEIGHTS_IN_CONSTRUCTION) {
			temp = m_assemblyMatrix * m_weightMatrix * m_assemblyMatrix.transpose();
		}
		else {
			temp = m_assemblyMatrix * m_assemblyMatrix.transpose();
		}
		m_lhsMatrixSubspace = posSubspaceMat_T * temp * posSubspaceMat;
		m_lhsMatrixSubspaceInitialized = true;
	}
	return m_lhsMatrixSubspace;
}

PDSparseMatrix PD::RHSInterpolationGroup::getLHSMatrix()
{
	PDSparseMatrix temp;
	if (NO_WEIGHTS_IN_CONSTRUCTION) {
		temp = m_assemblyMatrix * m_weightMatrix * m_assemblyMatrix.transpose();
	}
	else {
		temp = m_assemblyMatrix * m_assemblyMatrix.transpose();
	}

	return temp;
}

bool PD::RHSInterpolationGroup::hasBasis()
{
	return m_basis.cols() >= 1 && m_basis.rows() > 1;
}

std::vector<ProjDynConstraint*>& PD::RHSInterpolationGroup::getSampledConstraints()
{
	return m_sampledConstraints;
}

std::vector<ProjDynConstraint*>& PD::RHSInterpolationGroup::getConstraints()
{
	return m_constraints;
}

void PD::RHSInterpolationGroup::setExampleWeights(std::vector<PDScalar>& weights)
{
	if (m_isTetExampleGroup) {
		for (ProjDynConstraint* c : m_sampledConstraints) {
			TetExampleBased* tc = (TetExampleBased*)c;
			tc->setExampleWeights(weights);
		}
	}
	else if (getName() == "spring") {
		for (ProjDynConstraint* c : m_sampledConstraints) {
			SpringConstraint* sc = (SpringConstraint*)c;
			sc->setExWeights(weights);
		}
	}
	else {
		return;
	}
}

void PD::RHSInterpolationGroup::setWeightFactor(PDScalar w)
{
	m_weightFac = w;
}

void PD::RHSInterpolationGroup::setBlowup(PDScalar s)
{
	m_blowUp = s;
}

std::string PD::RHSInterpolationGroup::getName()
{
	return m_groupName;
}


// --------------------------------------------------------------------------
//                   OpenMS -- Open-Source Mass Spectrometry
// --------------------------------------------------------------------------
// Copyright The OpenMS Team -- Eberhard Karls University Tuebingen,
// ETH Zurich, and Freie Universitaet Berlin 2002-2017.
//
// This software is released under a three-clause BSD license:
//  * 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 any author or any participating institution
//    may be used to endorse or promote products derived from this software
//    without specific prior written permission.
// For a full list of authors, refer to the file AUTHORS.
// --------------------------------------------------------------------------
// 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 ANY OF THE AUTHORS OR THE CONTRIBUTING
// INSTITUTIONS 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.
//
// --------------------------------------------------------------------------
// $Maintainer: Chris Bielow $
// $Authors: Andreas Bertsch, Chris Bielow $
// --------------------------------------------------------------------------
//

#include <OpenMS/MATH/STATISTICS/GaussFitter.h>

#include <boost/math/distributions/normal.hpp>
#include <unsupported/Eigen/NonLinearOptimization>

using namespace std;

// #define GAUSS_FITTER_VERBOSE
// #undef  GAUSS_FITTER_VERBOSE

namespace OpenMS
{
  namespace Math
  {
    GaussFitter::GaussFitter()
    : init_param_(0.06, 3.0, 0.5)
    {
    }

    GaussFitter::~GaussFitter()
    {
    }

    void GaussFitter::setInitialParameters(const GaussFitResult & param)
    {
      init_param_ = param;
    }

    struct GaussFunctor
    {
      int inputs() const { return m_inputs; }
      int values() const { return m_values; }

      GaussFunctor(int dimensions, const std::vector<DPosition<2> >* data)
      : m_inputs(dimensions),
        m_values(static_cast<int>(data->size())),
        m_data(data)
      {}

      int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
      {
        const double A = x(0);
        const double x0 = x(1);
        const double sig = x(2);
        const double sig2 = 2 * sig * sig;

        UInt i = 0;
        for (std::vector<DPosition<2> >::const_iterator it = m_data->begin(); it != m_data->end(); ++it, ++i)
        {
          fvec(i) = A * std::exp(- (it->getX() - x0) * (it->getX() - x0) / sig2) - it->getY();
        }

        return 0;
      }
      // compute Jacobian matrix for the different parameters
      int df(const Eigen::VectorXd &x, Eigen::MatrixXd &J) const
      {
        const double A = x(0);
        const double x0 = x(1);
        const double sig = x(2);
        const double sig2 = 2 * sig * sig;
        const double sig3 = 2 * sig2 * sig;

        UInt i = 0;
        for (std::vector<DPosition<2> >::const_iterator it = m_data->begin(); it != m_data->end(); ++it, ++i)
        {
          const double xd = (it->getX() - x0);
          const double xd2 = xd*xd;
          double j0 = std::exp(-1.0 * xd2 / sig2);
          J(i,0) = j0;
          J(i,1) = (A * j0 * (-(-2 * it->getX() + 2.0 * x0) / sig2));
          J(i,2) = (A * j0 * (xd2 / sig3));
        }
        return 0;
      }

      const int m_inputs, m_values;
      const std::vector<DPosition<2> >* m_data;
    };

    GaussFitter::GaussFitResult GaussFitter::fit(vector<DPosition<2> > & input) const
    {
      Eigen::VectorXd x_init (3);
      x_init(0) = init_param_.A;
      x_init(1) = init_param_.x0;
      x_init(2) = init_param_.sigma;
      GaussFunctor functor (3, &input);
      Eigen::LevenbergMarquardt<GaussFunctor> lmSolver (functor);
      Eigen::LevenbergMarquardtSpace::Status status = lmSolver.minimize(x_init);

      // the states are poorly documented. after checking the source and
      // http://www.ultimatepp.org/reference%24Eigen_demo%24en-us.html we believe that
      // all states except TooManyFunctionEvaluation and ImproperInputParameters are good
      // termination states.
      if (status == Eigen::LevenbergMarquardtSpace::ImproperInputParameters ||
          status == Eigen::LevenbergMarquardtSpace::TooManyFunctionEvaluation)
      {
          throw Exception::UnableToFit(__FILE__, __LINE__, OPENMS_PRETTY_FUNCTION, "UnableToFit-GaussFitter", "Could not fit the Gaussian to the data: Error " + String(status));
      }

      x_init(2) = fabs(x_init(2)); // sigma can be negative, but |sigma| would actually be the correct solution

#ifdef GAUSS_FITTER_VERBOSE
      std::stringstream formula;
      formula << "f(x)=" << result.A << " * exp(-(x - " << result.x0 << ") ** 2 / 2 / (" << result.sigma << ") ** 2)";
      std::cout << formular.str() << std::endl;
#endif

      return GaussFitResult (x_init(0), x_init(1), x_init(2));
    }

    // static
    std::vector<double> GaussFitter::eval(const std::vector<double>& evaluation_points, const GaussFitter::GaussFitResult& model)
    {
      std::vector<double> out;
      out.reserve(evaluation_points.size());
      boost::math::normal_distribution<> ndf(model.x0, model.sigma);
      double int0 = model.A / boost::math::pdf(ndf, model.x0); // intensity normalization factor of the max @ x0 (simply multiplying the CDF with A is wrong!)
      for (Size i = 0; i < evaluation_points.size(); ++i)
      {
        out.push_back(boost::math::pdf(ndf, evaluation_points[i]) * int0 );
      }
      return out;
    }

  }   //namespace Math
} // namespace OpenMS