#ifndef HOPS_GAUSSIANPROCESS_HPP
#define HOPS_GAUSSIANPROCESS_HPP

#include <hops/RandomNumberGenerator/RandomNumberGenerator.hpp>

#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/SVD>

#include <random>
#include <vector>
#include <cmath>

#include <iostream>

namespace hops {
    namespace internal{
        template<typename MatrixType>
        MatrixType append(const MatrixType& rhs, const MatrixType& lhs) {
            MatrixType newRhs(rhs.rows() + lhs.rows(), lhs.cols());
            if (rhs.size() > 0) {
                newRhs << rhs, lhs;
            } else {
                newRhs << lhs;
            }
            return newRhs;
        }

        template<typename MatrixType, typename VectorType>
        MatrixType append(const MatrixType& rhs, const VectorType& lhs) {
            MatrixType newRhs(rhs.rows() + 1, lhs.cols());
            if (rhs.size() > 0) {
                newRhs << rhs, lhs.transpose();
            } else {
                newRhs << lhs.transpose();
            }
            return newRhs;
        }

        template<typename VectorType>
        VectorType append(const VectorType& rhs, double lhs) {
            VectorType newRhs(rhs.rows() + 1);
            if (rhs.size() > 0) {
                newRhs << rhs, lhs;
            } else {
                newRhs << lhs;
            }
            return newRhs;
        }
    }

    template<typename MatrixType, typename VectorType, typename Kernel>
    class GaussianProcess {
    public:
        GaussianProcess (Kernel kernel, double constantPriorMean = 0) :
                kernel(kernel), isNewObservations(true) {
            priorMeanFunction = [=](VectorType) -> double { return constantPriorMean; };
        }

        GaussianProcess (Kernel kernel, std::function<double (VectorType)> priorMeanFunction) :
                priorMeanFunction(priorMeanFunction),
                kernel(kernel),
                isNewObservations(true) {
            //
        }

        GaussianProcess getPriorCopy() {
            return GaussianProcess<MatrixType, VectorType, Kernel>(this->kernel, this->priorMeanFunction);
        }

        GaussianProcess getPosteriorCopy() {
            GaussianProcess<MatrixType, VectorType, Kernel> gp = this->getPriorCopy();
            gp.storedInputs = storedInputs;
            gp.inputPriorMean = inputPriorMean;
            //gp.inputAggregatedErrors = inputAggregatedErrors;
            gp.observationInputCovariance = observationInputCovariance;
            gp.inputCovariance = inputCovariance;

            gp.posteriorMean = posteriorMean;
            gp.posteriorCovariance = posteriorCovariance;
            gp.sqrtPosteriorCovariance = sqrtPosteriorCovariance;

            gp.isNewObservations = isNewObservations;
            gp.observedCovariance = observedCovariance;
            gp.sqrtObservedCovariance = sqrtObservedCovariance;

            gp.observedInputs = observedInputs;
            gp.observedValues = observedValues;
            gp.observedValueErrors = observedValueErrors;
            return gp;
        }

        /**
         *  Compute the posterior mean and covariance for the given input. This method checks, if any data has changed before
         *  recomputing depending quantities, which makes it rather safe to call it before sampling or querying any posterior data
         *
         */
        void computePosterior(const MatrixType& input) {
            bool isNewInput = (!storedInputs.size() || input != storedInputs);
            if (isNewObservations || isNewInput) {
                observationInputCovariance = kernel(observedInputs, input);
                //inputAggregatedErrors = aggregateErrors(input);

                if (isNewInput) {
                    inputCovariance = kernel(input, input);
                    inputPriorMean = priorMean(input);

                    storedInputs = input;
                }

                if (isNewObservations) {
                    // compute the cholesky factorization on the new observed covariance
                    sqrtObservedCovariance = observedCovariance.llt().matrixL();

                    assert(sqrtObservedCovariance.isLowerTriangular()
                            && "error computing the cholesky factorization of the observed covariance, check code.");

                    posteriorMean = sqrtObservedCovariance.template triangularView<Eigen::Lower>().solve(observedValues - priorMean(observedInputs));
                    posteriorMean = sqrtObservedCovariance.template triangularView<Eigen::Lower>().transpose().solve(posteriorMean);

                    posteriorCovariance = sqrtObservedCovariance.template triangularView<Eigen::Lower>().solve(observationInputCovariance);
                    posteriorCovariance = sqrtObservedCovariance.template triangularView<Eigen::Lower>().transpose().solve(posteriorCovariance);
                }

//#ifndef NDEBUG
//            MatrixType invObservedCovariance = observedCovariance.inverse();
//            MatrixType control = invObservedCovariance * (observedValues - priorMean(observedInputs));
//            control -= posteriorMean;
//            assert((control.size() == 0 || control.cwiseAbs().maxCoeff() < 1.e-5) && "computing the inverse for control might have failed.");
//#endif

                posteriorMean = inputPriorMean + observationInputCovariance.transpose() * posteriorMean;

//#ifndef NDEBUG
//            control = invObservedCovariance * observationInputCovariance;
//            control -= posteriorCovariance;
//            assert((control.size() == 0 || control.cwiseAbs().maxCoeff() < 1.e-5) && "computing the inverse for control might have failed.");
//#endif

                posteriorCovariance = inputCovariance - observationInputCovariance.transpose() * posteriorCovariance;
                //posteriorCovariance += inputAggregatedErrors.asDiagonal();

                Eigen::BDCSVD<MatrixType> solver(MatrixType(posteriorCovariance), Eigen::ComputeFullU);
                sqrtPosteriorCovariance = solver.matrixU() * solver.singularValues().cwiseSqrt().asDiagonal();

                isNewObservations = false;
            }
        }

        /**
         * Sample from posterior at x
         *
         */
        Eigen::VectorXd sample(const MatrixType& input,
                               hops::RandomNumberGenerator& randomNumberGenerator,
                               size_t& maxElement) {
            computePosterior(input);

            VectorType draw(input.rows());
            auto standardNormal = std::normal_distribution<double>();

            for (long i = 0; i < input.rows(); ++i) {
                draw(i) = standardNormal(randomNumberGenerator);
                assert(!std::isnan(draw(i)));
            }

            draw = posteriorMean + sqrtPosteriorCovariance * draw;
            draw.maxCoeff(&maxElement);

            return draw;
        }

        Eigen::VectorXd sample(hops::RandomNumberGenerator& randomNumberGenerator,
                               size_t& maxElement) {
            return sample(storedInputs, randomNumberGenerator, maxElement);
        }

        Eigen::VectorXd sample(const MatrixType& x,
                               hops::RandomNumberGenerator& randomNumberGenerator) {
            size_t max;
            return sample(x, randomNumberGenerator, max);
        }

        Eigen::VectorXd sample(hops::RandomNumberGenerator& randomNumberGenerator) {
            size_t max;
            return sample(storedInputs, randomNumberGenerator, max);
        }

        /**
         *  Given an observation (x_i, y_i, eps_i) stored in *this, if there is an x_j in the argument x passed, s.t. x_i = x_j, then
         *  y_i := x_j and eps_i := y_j
         *
         *  All x_j and the respective y_j and eps_j from the passed arguments, which were not found in *this, will be stored in the reference arguments
         *  x, y and error.
         *
         *  The isUnique argument controls, whether x may be assumed to be unique in *this. If isUnique == true, then only the data at the first
         *  occurence of x_i in *this will be updated.
         */
        std::tuple<MatrixType, MatrixType, VectorType> updateObservations(const MatrixType& x,
                                                                          const VectorType& y,
                                                                          const VectorType& error,
                                                                          bool isUnique = false) {
            assert(x.size() == y.size());
            assert(y.size() == error.size());

            // collect indices of observations which should be updated
            std::vector<long> updateCandidates;
            std::vector<long> notFound;
            for (long i = 0; i < x.rows(); ++i) {
                bool foundInput = false;
                for (long j = 0; j < observedInputs.rows(); ++j) {
                    if (x.row(i) == observedInputs.row(j)) {
                        updateCandidates.push_back(j);
                        foundInput = true;

                        if (isUnique) {
                            break;
                        }
                    }
                }

                // record data which could not be updated, because it was not stored in *this the first place
                // this data is "returned" in the reference arguments, such that it can be added after this function returns
                if (!foundInput) {
                    notFound.push_back(i);
                }
            }

            // if any entries were updated, set the isNewObservations flag
            if (updateCandidates.size() > 0) {
                isNewObservations = true;
            }

            for (size_t i = 0; i < updateCandidates.size(); ++i) {
                size_t k = updateCandidates[i];

                // update the data
                observedValues(k) = y(i);
                observedValueErrors(k) = error(i);

                // update the observed covariance
                //observedCovariance.row(k) = kernel({observedInputs[k]}, observedInputs);
                //observedCovariance.col(k) = observedCovariance.row(k).transpose();
                observedCovariance(k, k) = kernel(observedInputs.row(k), observedInputs.row(k))(0, 0) + observedValueErrors(k);
                //observedCovariance(k, k) += 1.e-5;
            }

//#ifndef NDEBUG
//            MatrixType control = kernel(observedInputs, observedInputs);
//            control -= observedCovariance;
//            assert(control.size() == 0 || control.cwiseAbs().maxCoeff() < 1.e-5);
//#endif

            MatrixType inputsNotFound(notFound.size(), x.cols());
            VectorType valuesNotFound(notFound.size());
            VectorType errorsNotFound(notFound.size());

            for (size_t i = 0; i < notFound.size(); ++i) {
                long k = notFound[i];

                inputsNotFound.row(i) = x.row(k);
                valuesNotFound.row(i) = y.row(k);
                errorsNotFound.row(i) = error.row(k);
            }

            //x = inputsNotFound;
            //y = valuesNotFound;
            //error = errorsNotFound;
            return {inputsNotFound, valuesNotFound, errorsNotFound};
        }

        //void updateObservations(MatrixType& x,
        //                        VectorType& y,
        //                        VectorType& error,
        //                        bool isUnique = false) {
        //    std::tie(x, y, error) = updateObservationsConstArgs(x, y, error);
        //}

        void updateObservations(const MatrixType& x, const VectorType& y) {
            updateObservations(x, y, VectorType::Zeros(y.rows()));
        }

        //void updateObservation(const VectorType& x, double y, double error = 0) {
        //    updateObservations({x}, {y}, {error});
        //}

        void addObservations(const MatrixType& x, VectorType& y, VectorType& error) {
            assert(x.size() == y.size());
            assert(y.size() == error.size());

            isNewObservations = true;

            auto n = observedValues.size();
            auto m = x.size();

            //VectorType newObservedValues = VectorType(n + m);
            //newObservedValues << observedValues, y;
            observedValues = internal::append(observedValues, y);

            //VectorType newObservedValueErrors = VectorType(n + m);
            //newObservedValueErrors << observedValueErrors, error;
            observedValueErrors = internal::append(observedValueErrors, error);

            MatrixType newObservedCovariance = MatrixType::Zero(n + m, n + m);

            newObservedCovariance.block(0, 0, n, n) = observedCovariance; // hopefully saves some computation time
            newObservedCovariance.block(0, n, n, m) = kernel(observedInputs, x);
            newObservedCovariance.block(n, 0, m, n) = newObservedCovariance.block(0, n, n, m).transpose().eval();
            newObservedCovariance.block(n, n, m, m) = kernel(x, x);
            //newObservedCovariance.block(n, n, m, m).diagonal().array() += 1.e-5;

            newObservedCovariance.block(n, n, m, m) += Eigen::MatrixXd(error.asDiagonal());

            //MatrixType newObservedInputs = MatrixType::Zero(n + m, n + m);
            //newObservedInputs << observedInputs, x;
            observedInputs = internal::append(observedInputs, x);

#ifndef NDEBUG
            MatrixType control = kernel(observedInputs, observedInputs);
            control += Eigen::MatrixXd(observedValueErrors.asDiagonal());
            //control.diagonal().array() += 1.e-5;
            control -= newObservedCovariance;
            assert((control.size() == 0 || control.cwiseAbs().maxCoeff() < 1.e-5));
#endif

            //observedInputs = newObservedInputs;
            //observedValues = newObservedValues;
            //observedValueErrors = newObservedValueErrors;
            observedCovariance = newObservedCovariance;
            //observedCovariance = observedCovariance;
        }

        void addObservations(const MatrixType& x, const VectorType& y) {
            addObservations(x, y, VectorType::Zeros(y.rows()));
        }

        //void addObservation(const VectorType& x, double y, double error = 0) {
        //    addObservations({x}, {y}, {error});
        //}

        const VectorType& getPosteriorMean() {
            computePosterior(storedInputs);
            return posteriorMean;
        }

        const MatrixType& getPosteriorCovariance() {
            computePosterior(storedInputs);
            return posteriorCovariance;
        }

        const MatrixType& getSqrtPosteriorCovariance() {
            computePosterior(storedInputs);
            return sqrtPosteriorCovariance;
        }

        const MatrixType& getObservedInputs() const { return observedInputs; }
        const VectorType& getObservedValues() const { return observedValues; }
        const VectorType& getObservedValueErrors() const { return observedValueErrors; }

        const MatrixType& getObservedCovariance() const { return observedCovariance; }

        std::function<double (VectorType)>& getPriorMeanFunction() {
            return priorMeanFunction;
        }

        Kernel getKernel() {
            return kernel;
        }

    private:
        std::function<double (VectorType)> priorMeanFunction;
        Kernel kernel;

        MatrixType storedInputs;
        VectorType inputPriorMean;
        //VectorType inputAggregatedErrors;
        MatrixType observationInputCovariance;
        MatrixType inputCovariance;

        VectorType posteriorMean;
        MatrixType posteriorCovariance;
        MatrixType sqrtPosteriorCovariance;

        bool isNewObservations;
        MatrixType observedCovariance;
        MatrixType sqrtObservedCovariance;

        MatrixType observedInputs;
        VectorType observedValues;
        VectorType observedValueErrors;

        VectorType priorMean(const MatrixType& x) {
            VectorType prior(x.rows());
            for (long i = 0; i < x.rows(); ++i) {
                prior(i) = priorMeanFunction(x.row(i));
            }
            return prior;
        }

        //VectorType aggregateErrors(const std::vector<VectorType>& x) {
        //    VectorType errors(x.size());
        //    for (size_t i = 0; i < x.size(); ++i) {
        //        double mean = 0, error = 0;
        //        size_t count = 0;
        //
        //        for (size_t j = 0; j < observedInputs.size(); ++j) {
        //            if (x[i] == observedInputs[j]) {
        //                ++count;
        //                mean += observedValues(j);
        //                error += std::pow(observedValueErrors(j), 2) + std::pow(observedValues(j), 2);
        //            }
        //        }

        //        if (count > 0) {
        //            mean /= count;
        //            error /= count;
        //            error -= std::pow(mean, 2);
        //        }

        //        assert(error >= 0);
        //        errors(i) = error;
        //    }
        //    return errors;
        //}
    };
}

#endif // HOPS_GAUSSIANPROCESS_HPP


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

  @copyright 2016 NumScale SAS

  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_COMMON_SCALAR_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_REM_PIO2_STRAIGHT_HPP_INCLUDED

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/config.hpp>
#include <boost/simd/constant/one.hpp>
#include <boost/simd/constant/zero.hpp>
#include <boost/simd/detail/constant/pio2_1.hpp>
#include <boost/simd/detail/constant/pio2_2.hpp>
#include <boost/simd/constant/pio2_3.hpp>
#include <boost/simd/constant/pio_4.hpp>
#include <boost/simd/detail/dispatch/meta/as_integer.hpp>
#include <boost/simd/function/if_else.hpp>
#include <boost/simd/function/is_greater.hpp>
#include <boost/simd/function/genmask.hpp>
#include <boost/simd/function/multiplies.hpp>
#include <boost/simd/function/minus.hpp>
#include <utility>

namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD (rem_pio2_straight_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_ < bd::floating_<A0> >
                          )
  {
    using result_t = std::pair<A0, A0>;
    BOOST_FORCEINLINE result_t operator() ( A0 const& x) const BOOST_NOEXCEPT
    {
      if (x < Pio_4<A0>())
        return {Zero<A0>(), x};
      A0 xr = x-Pio2_1<A0>();
      xr -= Pio2_2<A0>();
      xr -= Pio2_3<A0>();
      return { One<A0>(), xr};
    }
  };

} } }

#endif


/*
 *
 * Copyright (c) Toon Knapen & Kresimir Fresl 2003
 *
 * 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)
 *
 * KF acknowledges the support of the Faculty of Civil Engineering,
 * University of Zagreb, Croatia.
 *
 */

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_GESVD_HPP
#define BOOST_NUMERIC_BINDINGS_LAPACK_GESVD_HPP

#include <boost/numeric/bindings/traits/type_traits.hpp>
#include <boost/numeric/bindings/traits/traits.hpp>
#include <boost/numeric/bindings/lapack/lapack.h>
#include <boost/numeric/bindings/traits/detail/array.hpp>
#include <boost/numeric/bindings/traits/detail/utils.hpp>

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
#  include <boost/static_assert.hpp>
#  include <boost/type_traits/is_same.hpp>
#endif

#include <cassert>

namespace boost { namespace numeric { namespace bindings {

  namespace lapack {

    ///////////////////////////////////////////////////////////////////
    //
    // singular value decomposition
    //
    ///////////////////////////////////////////////////////////////////

    /*
     * (simple driver)
     * gesvd() computes the singular value decomposition (SVD) of
     * M-by-N matrix A, optionally computing the left and/or right
     * singular vectors. The SVD is written
     *
     *     A = U * S * V^T    or    A = U * S * V^H
     *
     * where S is an M-by-N matrix which is zero except for its min(m,n)
     * diagonal elements, U is an M-by-M orthogonal/unitary matrix, and V
     * is an N-by-N orthogonal/unitary matrix. The diagonal elements of S
     * are the singular values of A; they are real and non-negative, and
     * are returned in descending  order. The first min(m,n) columns of
     * U and V are the left and right singular vectors of A. (Note that
     * the routine returns V^T or V^H, not V.
     */

    namespace detail {

      inline
      void gesvd (char const jobu, char const jobvt,
                  int const m, int const n, float* a, int const lda,
                  float* s, float* u, int const ldu,
                  float* vt, int const ldvt,
                  float* work, int const lwork, float* /* dummy */,
                  int* info)
      {
        LAPACK_SGESVD (&jobu, &jobvt, &m, &n, a, &lda,
                       s, u, &ldu, vt, &ldvt, work, &lwork, info);
      }

      inline
      void gesvd (char const jobu, char const jobvt,
                  int const m, int const n, double* a, int const lda,
                  double* s, double* u, int const ldu,
                  double* vt, int const ldvt,
                  double* work, int const lwork, double* /* dummy */,
                  int* info)
      {
        LAPACK_DGESVD (&jobu, &jobvt, &m, &n, a, &lda,
                       s, u, &ldu, vt, &ldvt, work, &lwork, info);
      }

      inline
      void gesvd (char const jobu, char const jobvt,
                  int const m, int const n,
                  traits::complex_f* a, int const lda,
                  float* s, traits::complex_f* u, int const ldu,
                  traits::complex_f* vt, int const ldvt,
                  traits::complex_f* work, int const lwork,
                  float* rwork, int* info)
      {
        LAPACK_CGESVD (&jobu, &jobvt, &m, &n,
                       traits::complex_ptr (a), &lda, s,
                       traits::complex_ptr (u), &ldu,
                       traits::complex_ptr (vt), &ldvt,
                       traits::complex_ptr (work), &lwork, rwork, info);
      }

      inline
      void gesvd (char const jobu, char const jobvt,
                  int const m, int const n,
                  traits::complex_d* a, int const lda,
                  double* s, traits::complex_d* u, int const ldu,
                  traits::complex_d* vt, int const ldvt,
                  traits::complex_d* work, int const lwork,
                  double* rwork, int* info)
      {
        LAPACK_ZGESVD (&jobu, &jobvt, &m, &n,
                       traits::complex_ptr (a), &lda, s,
                       traits::complex_ptr (u), &ldu,
                       traits::complex_ptr (vt), &ldvt,
                       traits::complex_ptr (work), &lwork, rwork, info);
      }

      inline
      int gesvd_min_work (float, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m3x = 3 * minmn + maxmn;
        int m5 = 5 * minmn;
        return m3x < m5 ? m5 : m3x;
      }
      inline
      int gesvd_min_work (double, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        int m3x = 3 * minmn + maxmn;
        int m5 = 5 * minmn;
        return m3x < m5 ? m5 : m3x;
      }
      inline
      int gesvd_min_work (traits::complex_f, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        return 2 * minmn + maxmn;
      }
      inline
      int gesvd_min_work (traits::complex_d, int m, int n) {
        int minmn = m < n ? m : n;
        int maxmn = m < n ? n : m;
        return 2 * minmn + maxmn;
      }

      inline
      int gesvd_rwork (float, int, int) { return 1; }
      inline
      int gesvd_rwork (double, int, int) { return 1; }
      inline
      int gesvd_rwork (traits::complex_f, int m, int n) {
        return 5 * (m < n ? m : n);
      }
      inline
      int gesvd_rwork (traits::complex_d, int m, int n) {
        return 5 * (m < n ? m : n);
      }

    } // detail

    template <typename MatrA>
    inline
    int gesvd_work (char const q,
                    char const jobu, char const jobvt, MatrA const& a)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

#ifdef BOOST_NUMERIC_BINDINGS_LAPACK_2
      assert (q == 'M');
#else
      assert (q == 'M' || q == 'O');
#endif
      assert (jobu == 'N' || jobu == 'O' || jobu == 'A' || jobu == 'S');
      assert (jobvt == 'N' || jobvt == 'O' || jobvt == 'A' || jobvt == 'S');
      assert (!(jobu == 'O' && jobvt == 'O'));

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
      int lw = -13;

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif

      if (q == 'M')
        lw = detail::gesvd_min_work (val_t(), m, n);

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2
      MatrA& a2 = const_cast<MatrA&> (a);
      if (q == 'O') {
        // traits::detail::array<val_t> w (0);
        val_t w;
        int info;
        detail::gesvd (jobu, jobvt, m, n,
                       traits::matrix_storage (a2),
                       traits::leading_dimension (a2),
                       0, // traits::vector_storage (s),
                       0, // traits::matrix_storage (u),
                       m, // traits::leading_dimension (u),
                       0, // traits::matrix_storage (vt),
                       n, // traits::leading_dimension (vt),
                       &w, // traits::vector_storage (w),
                       -1, // traits::vector_size (w),
                       0, // traits::vector_storage (rw),
                       &info);
        assert (info == 0);
        lw = traits::detail::to_int (w);  // (w[0]);
      }
#endif

      return lw;
    }

    template <typename MatrA>
    inline
    int gesvd_rwork (MatrA const& a) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif

      return detail::gesvd_rwork (val_t(),
                                  traits::matrix_size1 (a),
                                  traits::matrix_size2 (a));
    }

    template <typename MatrA, typename VecS,
              typename MatrU, typename MatrV, typename VecW>
    inline
    int gesvd (char const jobu, char const jobvt,
               MatrA& a, VecS& s, MatrU& u, MatrV& vt, VecW& w)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));

      BOOST_STATIC_ASSERT(
        (boost::is_same<
          typename traits::matrix_traits<MatrA>::value_type, float
        >::value
        ||
        boost::is_same<
          typename traits::matrix_traits<MatrA>::value_type, double
        >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
#ifndef NDEBUG /* this variable is only used in assertions below */
      int const minmn = m < n ? m : n;
#endif

      assert (minmn == traits::vector_size (s));
      assert (!(jobu == 'O' && jobvt == 'O'));
      assert ((jobu == 'N')
              || (jobu == 'O')
              || (jobu == 'A' && m == traits::matrix_size2 (u))
              || (jobu == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobu == 'N' && traits::leading_dimension (u) >= 1)
              || (jobu == 'O' && traits::leading_dimension (u) >= 1)
              || (jobu == 'A' && traits::leading_dimension (u) >= m)
              || (jobu == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobvt == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobvt == 'O' && traits::leading_dimension (vt) >= 1)
              || (jobvt == 'A' && traits::leading_dimension (vt) >= n)
              || (jobvt == 'S' && traits::leading_dimension (vt) >= minmn));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      assert (traits::vector_size(w) >= detail::gesvd_min_work(val_t(),m,n));

      int info;
      detail::gesvd (jobu, jobvt, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     0, // dummy argument
                     &info);
      return info;
    }

    template <typename MatrA, typename VecS,
              typename MatrU, typename MatrV, typename VecW, typename VecRW>
    inline
    int gesvd (char const jobu, char const jobvt,
               MatrA& a, VecS& s, MatrU& u, MatrV& vt, VecW& w, VecRW& rw)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
#ifndef NDEBUG /* this variable is only used in assertions below */
      int const minmn = m < n ? m : n;
#endif

      assert (minmn == traits::vector_size (s));
      assert (!(jobu == 'O' && jobvt == 'O'));
      assert ((jobu == 'N')
              || (jobu == 'O')
              || (jobu == 'A' && m == traits::matrix_size2 (u))
              || (jobu == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobu == 'N' && traits::leading_dimension (u) >= 1)
              || (jobu == 'O' && traits::leading_dimension (u) >= 1)
              || (jobu == 'A' && traits::leading_dimension (u) >= m)
              || (jobu == 'S' && traits::leading_dimension (u) >= m));
      assert (n == traits::matrix_size2 (vt));
      assert ((jobvt == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobvt == 'O' && traits::leading_dimension (vt) >= 1)
              || (jobvt == 'A' && traits::leading_dimension (vt) >= n)
              || (jobvt == 'S' && traits::leading_dimension (vt) >= minmn));
#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      assert (traits::vector_size(w) >= detail::gesvd_min_work(val_t(),m,n));
      assert (traits::vector_size(rw) >= detail::gesvd_rwork(val_t(),m,n));

      int info;
      detail::gesvd (jobu, jobvt, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     &info);
      return info;
    }

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesvd (char const opt, char const jobu, char const jobvt,
               MatrA& a, VecS& s, MatrU& u, MatrV& vt)
    {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrU>::matrix_structure,
        traits::general_t
      >::value));
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrV>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
#ifndef NDEBUG /* this variable is only used in assertions below */
      int const minmn = m < n ? m : n;
#endif

      assert (minmn == traits::vector_size (s));
      assert (!(jobu == 'O' && jobvt == 'O'));
      assert ((jobu == 'N')
              || (jobu == 'O')
              || (jobu == 'A' && m == traits::matrix_size2 (u))
              || (jobu == 'S' && minmn == traits::matrix_size2 (u)));
      assert ((jobu == 'N' && traits::leading_dimension (u) >= 1)
              || (jobu == 'O' && traits::leading_dimension (u) >= 1)
              || (jobu == 'A' && traits::leading_dimension (u) >= m)
              || (jobu == 'S' && traits::leading_dimension (u) >= m));
      assert ((jobvt == 'N' || traits::matrix_size2(vt) == n)) ;
      assert ((jobvt == 'N' && traits::leading_dimension (vt) >= 1)
              || (jobvt == 'O' && traits::leading_dimension (vt) >= 1)
              || (jobvt == 'A' && traits::leading_dimension (vt) >= n)
              || (jobvt == 'S' && traits::leading_dimension (vt) >= minmn));

#ifdef BOOST_NUMERIC_BINDINGS_LAPACK_2
      assert (opt == 'M');
#else
      assert (opt == 'M' || opt == 'O');
#endif

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;

      int const lw = gesvd_work (opt, jobu, jobvt, a);
      traits::detail::array<val_t> w (lw);
      if (!w.valid()) return -101;

      int const lrw = gesvd_rwork (a);
      traits::detail::array<real_t> rw (lrw);
      if (!rw.valid()) return -102;

      int info;
      detail::gesvd (jobu, jobvt, m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     traits::matrix_storage (u),
                     traits::leading_dimension (u),
                     traits::matrix_storage (vt),
                     traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     &info);
      return info;
    }

#ifndef BOOST_NUMERIC_BINDINGS_LAPACK_2

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesvd (char const jobu, char const jobvt,
               MatrA& a, VecS& s, MatrU& u, MatrV& vt)
    {
      return gesvd ('O', jobu, jobvt, a, s, u, vt);
    }

    template <typename MatrA, typename VecS, typename MatrU, typename MatrV>
    inline
    int gesvd (MatrA& a, VecS& s, MatrU& u, MatrV& vt) {
      return gesvd ('O', 'S', 'S', a, s, u, vt);
    }

    template <typename MatrA, typename VecS>
    inline
    int gesvd (MatrA& a, VecS& s) {

#ifndef BOOST_NUMERIC_BINDINGS_NO_STRUCTURE_CHECK
      BOOST_STATIC_ASSERT((boost::is_same<
        typename traits::matrix_traits<MatrA>::matrix_structure,
        traits::general_t
      >::value));
#endif

      int const m = traits::matrix_size1 (a);
      int const n = traits::matrix_size2 (a);
#ifndef NDEBUG /* this variable is only used in assertions below */
      int const minmn = m < n ? m : n;
#endif

      assert (minmn == traits::vector_size (s));

#ifndef BOOST_NUMERIC_BINDINGS_POOR_MANS_TRAITS
      typedef typename traits::matrix_traits<MatrA>::value_type val_t;
#else
      typedef typename MatrA::value_type val_t;
#endif
      typedef typename traits::type_traits<val_t>::real_type real_t;

      int const lw = gesvd_work ('O', 'N', 'N', a);
      traits::detail::array<val_t> w (lw);
      if (!w.valid()) return -101;

      int const lrw = gesvd_rwork (a);
      traits::detail::array<real_t> rw (lrw);
      if (!rw.valid()) return -102;

      int info;
      detail::gesvd ('N', 'N', m, n,
                     traits::matrix_storage (a),
                     traits::leading_dimension (a),
                     traits::vector_storage (s),
                     0, // traits::matrix_storage (u),
                     1, // traits::leading_dimension (u),
                     0, // traits::matrix_storage (vt),
                     1, // traits::leading_dimension (vt),
                     traits::vector_storage (w),
                     traits::vector_size (w),
                     traits::vector_storage (rw),
                     &info);
      return info;
    }

#endif

  } // namespace lapack

}}}

#endif


 /*
 *  Copyright 2007-2015 The OpenMx Project
 *
 *  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.
 */

#include "omxFitFunction.h"
#include "omxGREMLfitfunction.h"
#include "omxGREMLExpectation.h"
#include <Eigen/Core>
#include <Eigen/Cholesky>
#include <Eigen/Dense>

struct omxGREMLFitState {
  //TODO(?): Some of these members might be redundant with what's stored in the FitContext,
  //and could therefore be cut
  omxMatrix *y, *X, *cov, *invcov, *means;
  std::vector< omxMatrix* > dV;
  std::vector< const char* > dVnames;
  int dVlength, usingGREMLExpectation;
  double nll, REMLcorrection;
  Eigen::VectorXd gradient;
  Eigen::MatrixXd avgInfo; //the Average Information matrix
  FreeVarGroup *varGroup;
	std::vector<int> gradMap;
  void buildParamMap(FreeVarGroup *newVarGroup);
};

void omxInitGREMLFitFunction(omxFitFunction *oo){

  if(OMX_DEBUG) { mxLog("Initializing GREML fitfunction."); }
  SEXP rObj = oo->rObj;
  SEXP dV, dVnames;
  int i=0;

  oo->units = FIT_UNITS_MINUS2LL;
  oo->computeFun = omxCallGREMLFitFunction;
  oo->ciFun = loglikelihoodCIFun;
  oo->destructFun = omxDestroyGREMLFitFunction;
  oo->populateAttrFun = omxPopulateGREMLAttributes;

  omxGREMLFitState *newObj = new omxGREMLFitState;
  oo->argStruct = (void*)newObj;
  omxExpectation* expectation = oo->expectation;
  omxState* currentState = expectation->currentState;
  newObj->usingGREMLExpectation = (strcmp(expectation->expType, "MxExpectationGREML")==0 ? 1 : 0);
  if(!newObj->usingGREMLExpectation){
    //Maybe someday GREML fitfunction could be made compatible with another expectation, but not at present:
    Rf_error("GREML fitfunction is currently only compatible with GREML expectation");
  }
  else{
    omxGREMLExpectation* oge = (omxGREMLExpectation*)(expectation->argStruct);
    oge->alwaysComputeMeans = 0;
  }

  newObj->y = omxGetExpectationComponent(expectation, oo, "y");
  newObj->cov = omxGetExpectationComponent(expectation, oo, "cov");
  newObj->invcov = omxGetExpectationComponent(expectation, oo, "invcov");
  newObj->X = omxGetExpectationComponent(expectation, oo, "X");
  newObj->means = omxGetExpectationComponent(expectation, oo, "means");
  newObj->nll = 0;
  newObj->REMLcorrection = 0;
  newObj->varGroup = NULL;

  //Derivatives:
  {ScopedProtect p1(dV, R_do_slot(rObj, Rf_install("dV")));
  ScopedProtect p2(dVnames, R_do_slot(rObj, Rf_install("dVnames")));
  newObj->dVlength = Rf_length(dV);
  newObj->dV.resize(newObj->dVlength);
  newObj->dVnames.resize(newObj->dVlength);
	if(newObj->dVlength){
    if(!newObj->usingGREMLExpectation){
      //Probably best not to allow use of dV if we aren't sure means will be calculated GREML-GLS way:
      Rf_error("derivatives of 'V' matrix in GREML fitfunction only compatible with GREML expectation");
    }
    if(OMX_DEBUG) { mxLog("Processing derivatives of V."); }
		int* dVint = INTEGER(dV);
    for(i=0; i < newObj->dVlength; i++){
      newObj->dV[i] = omxMatrixLookupFromState1(dVint[i], currentState);
      SEXP elem;
      {ScopedProtect p3(elem, STRING_ELT(dVnames, i));
			newObj->dVnames[i] = CHAR(elem);}
	}}
  }

  if(newObj->dVlength){
    oo->gradientAvailable = true;
    newObj->gradient.setZero(newObj->dVlength,1);
    oo->hessianAvailable = true;
    newObj->avgInfo.setZero(newObj->dVlength,newObj->dVlength);
    for(i=0; i < newObj->dVlength; i++){
      if( (newObj->dV[i]->rows != newObj->cov->rows) || (newObj->dV[i]->cols != newObj->cov->cols) ){
        Rf_error("all derivatives of V must have the same dimensions as V");
}}}}

void omxCallGREMLFitFunction(omxFitFunction *oo, int want, FitContext *fc){
  if (want & (FF_COMPUTE_PREOPTIMIZE)) return;

  //Recompute Expectation:
  omxExpectation* expectation = oo->expectation;
  omxExpectationCompute(expectation, NULL);

  omxGREMLFitState *gff = (omxGREMLFitState*)oo->argStruct; //<--Cast generic omxFitFunction to omxGREMLFitState

  //Ensure that the pointer in the GREML fitfunction is directed at the right FreeVarGroup
  //(not necessary for most compute plans):
  if(fc && gff->varGroup != fc->varGroup){
    gff->buildParamMap(fc->varGroup);
	}

  //Declare local variables used in more than one scope in this function:
  const double Scale = fabs(Global->llScale); //<--absolute value of loglikelihood scale
  const double NATLOG_2PI = 1.837877066409345483560659472811;	//<--log(2*pi)
  int i;
  Eigen::Map< Eigen::MatrixXd > Eigy(omxMatrixDataColumnMajor(gff->y), gff->y->cols, 1);
  Eigen::Map< Eigen::MatrixXd > Vinv(omxMatrixDataColumnMajor(gff->invcov), gff->invcov->rows, gff->invcov->cols);
  EigenMatrixAdaptor EigX(gff->X);
  Eigen::MatrixXd P, Py;
  P.setZero(gff->invcov->rows, gff->invcov->cols);
  double logdetV=0, logdetquadX=0, ytPy=0;

  if(want & (FF_COMPUTE_FIT | FF_COMPUTE_GRADIENT | FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
    if(gff->usingGREMLExpectation){
      omxGREMLExpectation* oge = (omxGREMLExpectation*)(expectation->argStruct);

      //Check that factorizations of V and the quadratic form in X succeeded:
      if(oge->cholV_fail_om->data[0]){
        oo->matrix->data[0] = NA_REAL;
        if (fc) fc->recordIterationError("expected covariance matrix is non-positive-definite");
        return;
      }
      if(oge->cholquadX_fail){
        oo->matrix->data[0] = NA_REAL;
        if (fc) fc->recordIterationError("Cholesky factorization failed; possibly, the matrix of covariates is rank-deficient");
        return;
      }

      //Log determinant of V:
      logdetV = oge->logdetV_om->data[0];

      //Log determinant of quadX:
      for(i=0; i < gff->X->cols; i++){
        logdetquadX += log(oge->cholquadX_vectorD[i]);
      }
      logdetquadX *= 2;
      gff->REMLcorrection = Scale*0.5*logdetquadX;

      //Finish computing fit (negative loglikelihood):
      P.triangularView<Eigen::Lower>() = (Vinv.selfadjointView<Eigen::Lower>() * //P = Vinv * (I-Hatmat)
        (Eigen::MatrixXd::Identity(Vinv.rows(), Vinv.cols()) -
          (EigX * oge->quadXinv.selfadjointView<Eigen::Lower>() * oge->XtVinv))).triangularView<Eigen::Lower>();
      Py = P.selfadjointView<Eigen::Lower>() * Eigy;
      ytPy = (Eigy.transpose() * Py)(0,0);
      if(OMX_DEBUG) {mxLog("ytPy is %3.3f",ytPy);}
      oo->matrix->data[0] = gff->REMLcorrection + Scale*0.5*( (((double)gff->y->cols) * NATLOG_2PI) + logdetV + ytPy);
      gff->nll = oo->matrix->data[0];
    }
    else{ //If not using GREML expectation, deal with means and cov in a general way to compute fit...
      //Declare locals:
      EigenMatrixAdaptor yhat(gff->means);
      EigenMatrixAdaptor EigV(gff->cov);
      double logdetV=0, logdetquadX=0;
      Eigen::MatrixXd Vinv, quadX;
      Eigen::LLT< Eigen::MatrixXd > cholV(gff->cov->rows);
      Eigen::LLT< Eigen::MatrixXd > cholquadX(gff->X->cols);
      Eigen::VectorXd cholV_vectorD, cholquadX_vectorD;

      //Cholesky factorization of V:
      cholV.compute(EigV);
      if(cholV.info() != Eigen::Success){
        omxRaiseErrorf("expected covariance matrix is non-positive-definite");
        oo->matrix->data[0] = NA_REAL;
        return;
      }
      //Log determinant of V:
      cholV_vectorD = (( Eigen::MatrixXd )(cholV.matrixL())).diagonal();
      for(i=0; i < gff->X->rows; i++){
        logdetV += log(cholV_vectorD[i]);
      }
      logdetV *= 2;

      Vinv = cholV.solve(Eigen::MatrixXd::Identity( EigV.rows(), EigV.cols() )); //<-- V inverse

      quadX = EigX.transpose() * Vinv * EigX; //<--Quadratic form in X

      cholquadX.compute(quadX); //<--Cholesky factorization of quadX
      if(cholquadX.info() != Eigen::Success){
        omxRaiseErrorf("Cholesky factorization failed; possibly, the matrix of covariates is rank-deficient");
        oo->matrix->data[0] = NA_REAL;
        return;
      }
      cholquadX_vectorD = (( Eigen::MatrixXd )(cholquadX.matrixL())).diagonal();
      for(i=0; i < gff->X->cols; i++){
        logdetquadX += log(cholquadX_vectorD[i]);
      }
      logdetquadX *= 2;
      gff->REMLcorrection = Scale*0.5*logdetquadX;

      //Finish computing fit:
      oo->matrix->data[0] = gff->REMLcorrection + Scale*0.5*( ((double)gff->y->rows * NATLOG_2PI) + logdetV +
        ( Eigy.transpose() * Vinv * (Eigy - yhat) )(0,0));
      gff->nll = oo->matrix->data[0];
      return;
    }
  }

  if(want & (FF_COMPUTE_GRADIENT | FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
    //This part requires GREML expectation:
    omxGREMLExpectation* oge = (omxGREMLExpectation*)(expectation->argStruct);

    //Declare local variables for this scope:
    int nThreadz = Global->numThreads;

    fc->grad.resize(gff->dVlength); //<--Resize gradient in FitContext

    //Set up new HessianBlock:
    HessianBlock *hb = new HessianBlock;
    if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
      hb->vars.resize(gff->dVlength);
      hb->mat.resize(gff->dVlength, gff->dVlength);
    }

    //Begin looping thru free parameters:
#pragma omp parallel num_threads(nThreadz)
{
		int i=0, j=0, t1=0, t2=0;
		Eigen::MatrixXd PdV_dtheta1;
		Eigen::MatrixXd dV_dtheta1(Eigy.rows(), Eigy.rows()); //<--Derivative of V w/r/t parameter i.
		Eigen::MatrixXd dV_dtheta2(Eigy.rows(), Eigy.rows()); //<--Derivative of V w/r/t parameter j.
		int threadID = omx_absolute_thread_num();
		int istart = threadID * gff->dVlength / nThreadz;
		int iend = (threadID+1) * gff->dVlength / nThreadz;
		if(threadID == nThreadz-1){iend = gff->dVlength;}
		for(i=istart; i < iend; i++){
			t1 = gff->gradMap[i]; //<--Parameter number for parameter i.
			if(t1 < 0){continue;}
			if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){hb->vars[i] = t1;}
			if( oge->numcases2drop ){
				dropCasesAndEigenize(gff->dV[i], dV_dtheta1, oge->numcases2drop, oge->dropcase, 1);
			}
			else{dV_dtheta1 = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(gff->dV[i]), gff->dV[i]->rows, gff->dV[i]->cols);}
			//PdV_dtheta1 = P.selfadjointView<Eigen::Lower>() * dV_dtheta1.selfadjointView<Eigen::Lower>();
			PdV_dtheta1 = P.selfadjointView<Eigen::Lower>();
			PdV_dtheta1 = PdV_dtheta1 * dV_dtheta1.selfadjointView<Eigen::Lower>();
			for(j=i; j < gff->dVlength; j++){
				if(j==i){
					gff->gradient(t1) = Scale*0.5*(PdV_dtheta1.trace() - (Eigy.transpose() * PdV_dtheta1 * Py)(0,0));
					fc->grad(t1) += gff->gradient(t1);
					if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
						gff->avgInfo(t1,t1) = Scale*0.5*(Eigy.transpose() * PdV_dtheta1 * PdV_dtheta1 * Py)(0,0);
					}
				}
				else{if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
					t2 = gff->gradMap[j]; //<--Parameter number for parameter j.
					if(t2 < 0){continue;}
					if( oge->numcases2drop ){
						dropCasesAndEigenize(gff->dV[j], dV_dtheta2, oge->numcases2drop, oge->dropcase, 1);
					}
					else{dV_dtheta2 = Eigen::Map< Eigen::MatrixXd >(omxMatrixDataColumnMajor(gff->dV[j]), gff->dV[j]->rows, gff->dV[j]->cols);}
					gff->avgInfo(t1,t2) = Scale*0.5*(Eigy.transpose() * PdV_dtheta1 * P.selfadjointView<Eigen::Lower>() * dV_dtheta2.selfadjointView<Eigen::Lower>() * Py)(0,0);
					gff->avgInfo(t2,t1) = gff->avgInfo(t1,t2);
				}}}}
}
    //Assign upper triangle elements of avgInfo to the HessianBlock:
    if(want & (FF_COMPUTE_HESSIAN | FF_COMPUTE_IHESSIAN)){
      for (size_t d1=0, h1=0; h1 < gff->dV.size(); ++h1) {
		    for (size_t d2=0, h2=0; h2 <= h1; ++h2) {
				  	hb->mat(d2,d1) = gff->avgInfo(h2,h1);
				    ++d2;
        }
			  ++d1;
		  }
		  fc->queue(hb);
  }}
  return;
}

void omxDestroyGREMLFitFunction(omxFitFunction *oo){
  if(OMX_DEBUG) {mxLog("Freeing GREML FitFunction.");}
    if(oo->argStruct == NULL) return;
    omxGREMLFitState* owo = ((omxGREMLFitState*)oo->argStruct);
    delete owo;
}

static void omxPopulateGREMLAttributes(omxFitFunction *oo, SEXP algebra){
  if(OMX_DEBUG) { mxLog("Populating GREML Attributes."); }
  SEXP rObj = oo->rObj;
  SEXP nval, mlfitval;
  int userSuppliedDataNumObs = (int)(( (omxGREMLExpectation*)(oo->expectation->argStruct) )->data2->numObs);

  //Tell the frontend fitfunction counterpart how many observations there are...:
  {
  ScopedProtect p1(nval, R_do_slot(rObj, Rf_install("numObs")));
  int* numobs = INTEGER(nval);
  numobs[0] = 1L - userSuppliedDataNumObs;
  /*^^^^The end result is that number of observations will be reported as 1 in summary()...
  which is always correct with GREML*/
	}

	omxGREMLFitState *gff = (omxGREMLFitState*)oo->argStruct;
	{
	ScopedProtect p1(mlfitval, R_do_slot(rObj, Rf_install("MLfit")));
	double* mlfit = REAL(mlfitval);
	mlfit[0] = gff->nll - gff->REMLcorrection;
	}
}

void omxGREMLFitState::buildParamMap(FreeVarGroup *newVarGroup)
{
  if(OMX_DEBUG) { mxLog("Building parameter map for GREML fitfunction."); }
  varGroup = newVarGroup;
  std::vector< omxMatrix* > dV_temp = dV;
  std::vector< const char* > dVnames_temp = dVnames;
	gradMap.resize(dVlength);
	int gx=0;
	for (int vx=0; vx < int(varGroup->vars.size()); ++vx) {
		for (int nx=0; nx < dVlength; ++nx) {
			if (strEQ(dVnames_temp[nx], varGroup->vars[vx]->name)) {
				gradMap[gx] = vx;
				dV[gx] = dV_temp[nx];
				dVnames[gx] = dVnames_temp[nx]; //<--Probably not strictly necessary...
				++gx;
				break;
			}
		}
	}
	if (gx != dVlength) Rf_error("Problem in dVnames mapping");
}

omxMatrix* omxMatrixLookupFromState1(int matrix, omxState* os) {
  omxMatrix* output = NULL;
	if(matrix == NA_INTEGER){return NULL;}
	if (matrix >= 0) {
		output = os->algebraList[matrix];
	}
  else {
		output = os->matrixList[~matrix];
	}
	return output;
}


// Copyright Abel Sinkovics (abel@sinkovics.hu)  2012.
// 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)

#include <boost/metaparse/repeated.hpp>
#include <boost/metaparse/sequence.hpp>
#include <boost/metaparse/lit_c.hpp>
#include <boost/metaparse/last_of.hpp>
#include <boost/metaparse/space.hpp>
#include <boost/metaparse/int_.hpp>
#include <boost/metaparse/foldl_reject_incomplete_start_with_parser.hpp>
#include <boost/metaparse/one_of.hpp>
#include <boost/metaparse/get_result.hpp>
#include <boost/metaparse/token.hpp>
#include <boost/metaparse/entire_input.hpp>
#include <boost/metaparse/string.hpp>
#include <boost/metaparse/transform.hpp>
#include <boost/metaparse/always.hpp>
#include <boost/metaparse/build_parser.hpp>

#include <boost/mpl/apply_wrap.hpp>
#include <boost/mpl/front.hpp>
#include <boost/mpl/back.hpp>
#include <boost/mpl/plus.hpp>
#include <boost/mpl/minus.hpp>
#include <boost/mpl/times.hpp>
#include <boost/mpl/divides.hpp>
#include <boost/mpl/bool.hpp>
#include <boost/mpl/equal_to.hpp>
#include <boost/mpl/if.hpp>

using boost::metaparse::sequence;
using boost::metaparse::lit_c;
using boost::metaparse::last_of;
using boost::metaparse::space;
using boost::metaparse::repeated;
using boost::metaparse::build_parser;
using boost::metaparse::int_;
using boost::metaparse::foldl_reject_incomplete_start_with_parser;
using boost::metaparse::get_result;
using boost::metaparse::one_of;
using boost::metaparse::token;
using boost::metaparse::entire_input;
using boost::metaparse::transform;
using boost::metaparse::always;

using boost::mpl::apply_wrap1;
using boost::mpl::front;
using boost::mpl::back;
using boost::mpl::plus;
using boost::mpl::minus;
using boost::mpl::times;
using boost::mpl::divides;
using boost::mpl::if_;
using boost::mpl::bool_;
using boost::mpl::equal_to;

/*
 * The grammar
 *
 * expression ::= plus_exp
 * plus_exp ::= prod_exp ((plus_token | minus_token) prod_exp)*
 * prod_exp ::= value_exp ((mult_token | div_token) value_exp)*
 * value_exp ::= int_token | '_'
 */

typedef token<lit_c<'+'> > plus_token;
typedef token<lit_c<'-'> > minus_token;
typedef token<lit_c<'*'> > mult_token;
typedef token<lit_c<'/'> > div_token;

typedef token<int_> int_token;
typedef token<lit_c<'_'> > arg_token;

template <class T, char C>
struct is_c : bool_<T::type::value == C> {};

struct build_plus
{
  template <class A, class B>
  struct _plus
  {
    typedef _plus type;

    template <class T>
    struct apply :
      plus<typename apply_wrap1<A, T>::type, typename apply_wrap1<B, T>::type>
    {};
  };

  template <class A, class B>
  struct _minus
  {
    typedef _minus type;

    template <class T>
    struct apply :
      minus<typename apply_wrap1<A, T>::type, typename apply_wrap1<B, T>::type>
    {};
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '+'>::type,
      _plus<State, typename back<C>::type>,
      _minus<State, typename back<C>::type>
    >
  {};
};

struct build_mult
{
  template <class A, class B>
  struct _mult
  {
    typedef _mult type;

    template <class T>
    struct apply :
      times<typename apply_wrap1<A, T>::type, typename apply_wrap1<B, T>::type>
    {};
  };

  template <class A, class B>
  struct _div
  {
    typedef _div type;

    template <class T>
    struct apply :
      divides<
        typename apply_wrap1<A, T>::type,
        typename apply_wrap1<B, T>::type
      >
    {};
  };

  template <class State, class C>
  struct apply :
    if_<
      typename is_c<front<C>, '*'>::type,
      _mult<State, typename back<C>::type>,
      _div<State, typename back<C>::type>
    >
  {};
};

class build_value
{
private:
  template <class V>
  struct impl
  {
    typedef impl type;

    template <class T>
    struct apply : V {};
  };

public:
  typedef build_value type;

  template <class V>
  struct apply : impl<typename V::type> {};
};

struct arg
{
  typedef arg type;

  template <class T>
  struct apply
  {
    typedef T type;
  };
};

typedef
  one_of<transform<int_token, build_value>, always<arg_token, arg> >
  value_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<mult_token, div_token>, value_exp>,
    value_exp,
    build_mult
  >
  prod_exp;

typedef
  foldl_reject_incomplete_start_with_parser<
    sequence<one_of<plus_token, minus_token>, prod_exp>,
    prod_exp,
    build_plus
  >
  plus_exp;

typedef last_of<repeated<space>, plus_exp> expression;

typedef build_parser<entire_input<expression> > metafunction_parser;

#if BOOST_METAPARSE_STD < 2011

template <class Exp>
struct meta_lambda : apply_wrap1<metafunction_parser, Exp> {};

int main()
{
  using std::cout;
  using std::endl;
  using boost::metaparse::string;

  typedef meta_lambda<string<'1','3'> >::type metafunction_class_1;
  typedef meta_lambda<string<'2',' ','+',' ','3'> >::type metafunction_class_2;
  typedef meta_lambda<string<'2',' ','*',' ','2'> >::type metafunction_class_3;
  typedef
    meta_lambda<string<' ','1','+',' ','2','*','4','-','6','/','2'> >::type
    metafunction_class_4;
  typedef meta_lambda<string<'2',' ','*',' ','_'> >::type metafunction_class_5;

  typedef boost::mpl::int_<11> int11;

  cout
    << apply_wrap1<metafunction_class_1, int11>::type::value << endl
    << apply_wrap1<metafunction_class_2, int11>::type::value << endl
    << apply_wrap1<metafunction_class_3, int11>::type::value << endl
    << apply_wrap1<metafunction_class_4, int11>::type::value << endl
    << apply_wrap1<metafunction_class_5, int11>::type::value << endl
    ;
}

#else

#ifdef META_LAMBDA
  #error META_LAMBDA already defined
#endif
#define META_LAMBDA(exp) \
  apply_wrap1<metafunction_parser, BOOST_METAPARSE_STRING(#exp)>::type

int main()
{
  using std::cout;
  using std::endl;

  typedef META_LAMBDA(13) metafunction_class_1;
  typedef META_LAMBDA(2 + 3) metafunction_class_2;
  typedef META_LAMBDA(2 * 2) metafunction_class_3;
  typedef META_LAMBDA( 1+ 2*4-6/2) metafunction_class_4;
  typedef META_LAMBDA(2 * _) metafunction_class_5;

  typedef boost::mpl::int_<11> int11;

  cout
    << apply_wrap1<metafunction_class_1, int11>::type::value << endl
    << apply_wrap1<metafunction_class_2, int11>::type::value << endl
    << apply_wrap1<metafunction_class_3, int11>::type::value << endl
    << apply_wrap1<metafunction_class_4, int11>::type::value << endl
    << apply_wrap1<metafunction_class_5, int11>::type::value << endl
    ;
}

#endif


#include <pybind11/pybind11.h>
#include <pybind11/eigen.h>
#include <pybind11/numpy.h>
#include <fstream>
#include <iostream>
#include <cfloat>
#include <vector>
#include <Eigen/Dense>
using namespace std;
using namespace Eigen;

ArrayXd find_max(VectorXd mMat)
{
	VectorXd::Index maxRow;
	double max = mMat.maxCoeff(&maxRow);
	Matrix<bool, Dynamic, 1> MM = (mMat.array() == max).matrix();
	ArrayXd Index_max = ArrayXd::Zero(mMat.size());
	for (size_t i = 0; i < MM.rows(); i++)
	{
		if (MM(i))
		{
			Index_max[i] = 1.0;
		}

	}
	return Index_max;
}

MatrixXd select_M_PN(MatrixXd C, VectorXd P)
{
	MatrixXd res(int(P.sum()), int(P.sum())), res_ing(C.rows(), int(P.sum()));
	Eigen::Index j = 0;
	for (Eigen::Index i = 0; i < C.cols(); ++i)
	{
		if (P(i) > 0) res_ing.col(j++) = C.col(i);
	}
	j = 0;
	for (Eigen::Index i = 0; i < C.cols(); ++i)
	{
		if (P(i) > 0)
			res.row(j++) = res_ing.row(i);
	}

	return res;

}

MatrixXd select_V_PN(VectorXd V, VectorXd P)
{
	VectorXd res(int(P.sum()));
	Eigen::Index j = 0;
	for (Eigen::Index i = 0; i < V.size(); ++i)
	{
		if (P(i) > 0) res(j++) = V(i);
	}
	return res;

}

VectorXd lsqnonneg(MatrixXd C, VectorXd d, double tol)
/*
min_{x>0}||Cx-d||_2^2
Reference: Lawson and Hanson,
"Solving Least Squares Problems",
Prentice-Hall, 1974.
*/
{
	int n = C.rows();
	// Initialize vector of n zeros and Infs(to be used later)
	VectorXd nZeros = VectorXd::Zero(n);
	VectorXd wz = nZeros;
	//Initialize set of non - active columns to null
	VectorXd P = nZeros;
	// Initialize set of active columns to all
	// and the initial point to zeros
	VectorXd Z = VectorXd::Ones(n);
	VectorXd x = nZeros;
	VectorXd w = d - C * x;
	//Set up iteration criterion
	int outeriter = 0;
	int iter = 0;
	int itmax = 3 * n;
	// Outer loop to put variables into set to hold positive coefficients
	while (Z.sum() > 0 && (w.array() > tol).any())
	{
		outeriter += 1;
		VectorXd z = nZeros;
		wz = (wz.array()*(1-P.array())).matrix() - P* DBL_MAX;
		wz = (wz.array() *(1-Z.array()) + w.array()*Z.array()).matrix();
		ArrayXd t = find_max(wz);
		P = (P.array() * (1 - t) + t).matrix();
		Z = (Z.array()*(1 - t)).matrix();
		if (P.sum() == 1)
		{
			VectorXd::Index maxRow;
			P.maxCoeff(&maxRow);
			z(maxRow) = d(maxRow) / C(maxRow, maxRow);
		}
		if (P.sum() >= 2)
		{
			MatrixXd C_ing = select_M_PN(C, P);
			VectorXd d_ing = select_V_PN(d, P);
			VectorXd z_ing = C_ing.ldlt().solve(d_ing);
			Eigen::Index j = 0;
			for (size_t i = 0; i < P.size(); i++)
			{
				if (P(i) > 0) z(i) = z_ing(j++);
			}
		}
		while ((z.array()*P.array() + 1 - P.array() <= 0).any())
		{
			iter = iter + 1;
			if (iter > itmax)
			{
				cout << "optimfun:lsqnonneg: IterationCountExceeded" << endl;
				x = z;
				return x;
			}

			// Find indices where intermediate solution z is approximately negative
			vector<double> alpha_v;
			for (Eigen::Index i = 0; i < P.size(); i++)
			{
				if(z(i)<=0 && P(i)>0) alpha_v.push_back(x(i)/(x(i)-z(i)));
			}
			x = x + (*min_element(alpha_v.begin(), alpha_v.end())) * (z - x);
			for (size_t i = 0; i < Z.size(); i++)
			{
				if (abs(x(i)) < tol && P(i)>0)
				{
					Z(i) = 1;
				}
			}

			P = (1 - Z.array()).matrix();
			z = nZeros;
			if (P.sum() == 1)
			{
				VectorXd::Index maxRow;
				P.maxCoeff(&maxRow);
				z(maxRow) = d(maxRow) / C(maxRow, maxRow);
			}
			if (P.sum() >= 2)
			{
				MatrixXd C_ing = select_M_PN(C, P);
				VectorXd d_ing = select_V_PN(d, P);
				VectorXd z_ing = C_ing.ldlt().solve(d_ing);
				Eigen::Index j = 0;
				for (size_t i = 0; i < P.size(); i++)
				{
					if (P(i) > 0) z(i) = z_ing(j++);
				}
			}
		}
		x = z;
		w = d - C*x;
	}
	return x;
}

MatrixXd SNMF_inner(MatrixXd C_mat, MatrixXd B_mat, double tol)
{
	int n = B_mat.cols();
	for (Eigen::Index i = 0; i < n; i++)
	{
		B_mat.col(i) = lsqnonneg(C_mat, B_mat.col(i), tol);
	}
	return B_mat;
}

namespace py = pybind11;
PYBIND11_MODULE(libSNMF_inner,m)
{
m.doc() = "SNMF_inner iteration";
m.def("SNMF_inner", &SNMF_inner);
}


#include "davidson.h"

#include <Eigen/Dense>

size_t Davidson::diagonalize(
    const std::vector<double>& initial_vector, std::size_t max_iterations) {
  const double TOLERANCE = 1.0e-7;

  if (n == 1) {
    lowest_eigenvalue = diagonal[0];
    lowest_eigenvector = std::vector<double>(1, 1.0);
    diagonalized = true;
    return 0;
  }

  const std::size_t iterations = std::min(n, max_iterations);
  double lowest_eigenvalue = 0.0;
  double lowest_eigenvalue_prev = 0.0;
  double residual_norm = 0.0;

  Eigen::MatrixXd v = Eigen::MatrixXd::Zero(n, iterations);

  if (initial_vector.size() != n) {
    v(0, 0) = 1.0;  // Start from HF.
  } else {
    for (std::size_t i = 0; i < n; i++) v(i, 0) = initial_vector[i];
    v.col(0).normalize();
  }

  Eigen::MatrixXd Hv = Eigen::MatrixXd::Zero(n, iterations);
  Eigen::VectorXd w = Eigen::VectorXd::Zero(n);  // Lowest eigenvector so far.
  Eigen::VectorXd Hw = Eigen::VectorXd::Zero(n);
  Eigen::MatrixXd h_krylov = Eigen::MatrixXd::Zero(iterations, iterations);
  Eigen::MatrixXd h_overwrite;
  Eigen::VectorXd eigenvalues = Eigen::VectorXd::Zero(iterations);
  std::size_t len_work = 3 * iterations - 1;
  Eigen::VectorXd work(len_work);
  bool converged = false;
  std::vector<double> tmp_v(n);

  // Get diagonal elements.
  Eigen::VectorXd diag_elems(n);
  for (std::size_t i = 0; i < n; i++) diag_elems[i] = diagonal[i];

  // First iteration.
  for (std::size_t i = 0; i < n; i++) tmp_v[i] = v(i, 0);
  const auto& tmp_Hv = apply_hamiltonian(tmp_v);
  for (std::size_t i = 0; i < n; i++) Hv(i, 0) = tmp_Hv[i];
  lowest_eigenvalue = v.col(0).dot(Hv.col(0));
  h_krylov(0, 0) = lowest_eigenvalue;
  w = v.col(0);
  Hw = Hv.col(0);
  if (verbose) printf("Davidson Iteration #1. Eigenvalue: %#.15g\n", lowest_eigenvalue);

  residual_norm = 1.0;  // So at least one iteration is done.
  std::size_t n_iter = std::min(n, iterations);
  size_t n_diagonalize = 1;

  for (std::size_t it = 1; it < n_iter; it++) {
    // Compute residual.
    for (std::size_t j = 0; j < n; j++) {
      v(j, it) = (Hw(j, 0) - lowest_eigenvalue * w(j, 0)) / (lowest_eigenvalue - diag_elems(j));
      if (fabs(lowest_eigenvalue - diag_elems[j]) < 1.0e-8) v(j, it) = -1.0;
    }

    // If residual is small, converge.
    residual_norm = v.col(it).norm();
    if (residual_norm < 1.0e-6) converged = true;

    // Orthogonalize and normalize.
    for (std::size_t i = 0; i < it; i++) {
      double norm = v.col(it).dot(v.col(i));
      v.col(it) -= norm * v.col(i);
    }
    v.col(it).normalize();

    // Apply H once.
    for (std::size_t i = 0; i < n; i++) tmp_v[i] = v(i, it);
    const auto& tmp_Hv2 = apply_hamiltonian(tmp_v);
    for (std::size_t i = 0; i < n; i++) Hv(i, it) = tmp_Hv2[i];

    // Construct Krylow matrix and diagonalize.
    for (std::size_t i = 0; i <= it; i++) {
      h_krylov(i, it) = v.col(i).dot(Hv.col(it));
      h_krylov(it, i) = h_krylov(i, it);
    }

    len_work = 3 * it + 2;
    h_overwrite = h_krylov.leftCols(it + 1).topRows(it + 1);
    Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> eigenSolver(
        h_krylov.leftCols(it + 1).topRows(it + 1));
    const auto& eigenvalues = eigenSolver.eigenvalues();
    const auto& eigenvectors = eigenSolver.eigenvectors();
    lowest_eigenvalue = eigenvalues[0];
    std::size_t lowest_id = 0;
    for (std::size_t i = 1; i <= it; i++) {
      if (eigenvalues[i] < lowest_eigenvalue) {
        lowest_eigenvalue = eigenvalues[i];
        lowest_id = i;
      }
    }
    w = v.leftCols(it) * eigenvectors.col(lowest_id).topRows(it);
    Hw = Hv.leftCols(it) * eigenvectors.col(lowest_id).topRows(it);

    if (it > 1 && fabs(lowest_eigenvalue - lowest_eigenvalue_prev) < TOLERANCE) {
      converged = true;
      break;
    } else {
      lowest_eigenvalue_prev = lowest_eigenvalue;
      n_diagonalize++;
      if (verbose)
        printf("Davidson Iteration #%zu. Eigenvalue: %#.15g\n", n_diagonalize, lowest_eigenvalue);
    }

    if (converged) break;
  }

  this->lowest_eigenvalue = lowest_eigenvalue;
  lowest_eigenvector.resize(n);
  for (std::size_t i = 0; i < n; i++) lowest_eigenvector[i] = w(i);
  diagonalized = true;

  return n_diagonalize;
}


#include <armadillo>

#include "psi4/libmints/basisset.h"
#include "psi4/libmints/integral.h"
#include "psi4/libmints/matrix.h"
#include "psi4/libmints/mintshelper.h"
#include "psi4/libmints/vector3.h"

using namespace psi;

void AO_dipole_length(arma::cube &M_AO,
                      const arma::vec &origin,
                      SharedWavefunction wfn,
                      Options &options) {

    const Vector3 sm_origin(origin.memptr());

    const size_t nbf = wfn->basisset()->nbf();

    std::shared_ptr<IntegralFactory> intfactory = wfn->integral();

    std::vector<SharedMatrix> v_dipole;

    v_dipole.push_back(SharedMatrix(new Matrix("AO Mux", nbf, nbf)));
    v_dipole.push_back(SharedMatrix(new Matrix("AO Muy", nbf, nbf)));
    v_dipole.push_back(SharedMatrix(new Matrix("AO Muz", nbf, nbf)));

    std::shared_ptr<OneBodyAOInt> ints(intfactory->ao_dipole());
    ints->set_origin(sm_origin);
    ints->compute(v_dipole);

    M_AO.set_size(nbf, nbf, v_dipole.size());

    for (size_t c = 0; c < v_dipole.size(); c++) {
        SharedMatrix msm_dipole = v_dipole[c];
        arma::mat ma_dipole(msm_dipole->get_pointer(),
                            msm_dipole->rowdim(),
                            msm_dipole->coldim(),
                            false,
                            true);
        M_AO.slice(c) = ma_dipole;
    }

    return;
}

void AO_quadrupole_length(arma::cube &M_AO,
                          const arma::vec &origin,
                          SharedWavefunction wfn,
                          Options &options) {

    const Vector3 sm_origin(origin.memptr());

    const size_t nbf = wfn->basisset()->nbf();

    std::shared_ptr<IntegralFactory> intfactory = wfn->integral();

    std::vector<SharedMatrix> v_quadrupole;

    v_quadrupole.push_back(SharedMatrix(new Matrix("AO Quadrupole XX", nbf, nbf)));
    v_quadrupole.push_back(SharedMatrix(new Matrix("AO Quadrupole XY", nbf, nbf)));
    v_quadrupole.push_back(SharedMatrix(new Matrix("AO Quadrupole XZ", nbf, nbf)));
    v_quadrupole.push_back(SharedMatrix(new Matrix("AO Quadrupole YY", nbf, nbf)));
    v_quadrupole.push_back(SharedMatrix(new Matrix("AO Quadrupole YZ", nbf, nbf)));
    v_quadrupole.push_back(SharedMatrix(new Matrix("AO Quadrupole ZZ", nbf, nbf)));

    std::shared_ptr<OneBodyAOInt> ints(intfactory->ao_quadrupole());
    ints->set_origin(sm_origin);
    ints->compute(v_quadrupole);

    M_AO.set_size(nbf, nbf, v_quadrupole.size());

    for (size_t c = 0; c < v_quadrupole.size(); c++) {
        SharedMatrix msm_quadrupole = v_quadrupole[c];
        arma::mat ma_quadrupole(msm_quadrupole->get_pointer(),
                                msm_quadrupole->rowdim(),
                                msm_quadrupole->coldim(),
                                false,
                                true);
        M_AO.slice(c) = ma_quadrupole;
    }

    return;
}

void AO_multipole(arma::cube &M_AO,
                  const arma::uvec &order,
                  const arma::vec &origin,
                  SharedWavefunction wfn,
                  Options &options) {

    // TODO
    return;
}

void AO_dipole_velocity(arma::cube &D_AO, SharedWavefunction wfn, Options &options) {

    const size_t nbf = wfn->basisset()->nbf();

    std::shared_ptr<IntegralFactory> intfactory = wfn->integral();

    std::vector<SharedMatrix> v_nabla;

    v_nabla.push_back(SharedMatrix(new Matrix("AO Px", nbf, nbf)));
    v_nabla.push_back(SharedMatrix(new Matrix("AO Py", nbf, nbf)));
    v_nabla.push_back(SharedMatrix(new Matrix("AO Pz", nbf, nbf)));

    std::shared_ptr<OneBodyAOInt> ints(intfactory->ao_nabla());
    ints->compute(v_nabla);

    D_AO.set_size(nbf, nbf, v_nabla.size());

    for (size_t c = 0; c < v_nabla.size(); c++) {
        SharedMatrix msm_nabla = v_nabla[c];
        arma::mat ma_nabla(msm_nabla->get_pointer(),
                           msm_nabla->rowdim(),
                           msm_nabla->coldim(),
                           false,
                           true);
        D_AO.slice(c) = ma_nabla;
        // TODO check for antisymmetry due to imaginary operator
    }

    return;
}

void AO_angular_momentum(arma::cube &L_AO,
                         const arma::vec &origin,
                         SharedWavefunction wfn,
                         Options &options) {

    const Vector3 sm_origin(origin.memptr());

    const size_t nbf = wfn->basisset()->nbf();

    std::shared_ptr<IntegralFactory> intfactory = wfn->integral();

    std::vector<SharedMatrix> v_angmom;

    v_angmom.push_back(SharedMatrix(new Matrix("AO Lx", nbf, nbf)));
    v_angmom.push_back(SharedMatrix(new Matrix("AO Ly", nbf, nbf)));
    v_angmom.push_back(SharedMatrix(new Matrix("AO Lz", nbf, nbf)));

    std::shared_ptr<OneBodyAOInt> ints(intfactory->ao_angular_momentum());
    ints->set_origin(sm_origin);
    ints->compute(v_angmom);

    L_AO.set_size(nbf, nbf, v_angmom.size());

    for (size_t c = 0; c < v_angmom.size(); c++) {
        SharedMatrix msm_angmom = v_angmom[c];
        arma::mat ma_angmom(msm_angmom->get_pointer(),
                            msm_angmom->rowdim(),
                            msm_angmom->coldim(),
                            false,
                            true);
        L_AO.slice(c) = ma_angmom;
        // TODO check for antisymmetry due to imaginary operator
    }

    return;
}


/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2004, 2005, 2006, 2007, 2009 StatPro Italia srl
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

/*! \file blackscholesprocess.hpp
    \brief Black-Scholes processes
*/

#ifndef quantlib_black_scholes_process_hpp
#define quantlib_black_scholes_process_hpp

#include <ql/stochasticprocess.hpp>
#include <ql/processes/eulerdiscretization.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/termstructures/volatility/equityfx/localvoltermstructure.hpp>
#include <ql/quote.hpp>

namespace QuantLib {

    class LocalConstantVol;
    class LocalVolCurve;

    //! Generalized Black-Scholes stochastic process
    /*! This class describes the stochastic process \f$ S \f$ governed by
        \f[
            d\ln S(t) = (r(t) - q(t) - \frac{\sigma(t, S)^2}{2}) dt
                     + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class GeneralizedBlackScholesProcess : public StochasticProcess1D {
      public:
        GeneralizedBlackScholesProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& dividendTS,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
        //! \name StochasticProcess1D interface
        //@{
        Real x0() const;
        /*! \todo revise extrapolation */
        Real drift(Time t, Real x) const;
        /*! \todo revise extrapolation */
        Real diffusion(Time t, Real x) const;
        Real apply(Real x0, Real dx) const;
        /*! \warning in general raises a "not implemented" exception.
                     It should be rewritten to return the expectation E(S)
                     of the process, not exp(E(log S)).
        */
        Real expectation(Time t0, Real x0, Time dt) const;
        Real stdDeviation(Time t0, Real x0, Time dt) const;
        Real variance(Time t0, Real x0, Time dt) const;
        Real evolve(Time t0, Real x0, Time dt, Real dw) const;
        //@}
        Time time(const Date&) const;
        //! \name Observer interface
        //@{
        void update();
        //@}
        //! \name Inspectors
        //@{
        const Handle<Quote>& stateVariable() const;
        const Handle<YieldTermStructure>& dividendYield() const;
        const Handle<YieldTermStructure>& riskFreeRate() const;
        const Handle<BlackVolTermStructure>& blackVolatility() const;
        const Handle<LocalVolTermStructure>& localVolatility() const;
        //@}
      private:
        Handle<Quote> x0_;
        Handle<YieldTermStructure> riskFreeRate_, dividendYield_;
        Handle<BlackVolTermStructure> blackVolatility_;
        bool forceDiscretization_;
        mutable RelinkableHandle<LocalVolTermStructure> localVolatility_;
        mutable bool updated_, isStrikeIndependent_;
    };

    //! Black-Scholes (1973) stochastic process
    /*! This class describes the stochastic process \f$ S \f$ for a stock
        given by
        \f[
            d\ln S(t) = (r(t) - \frac{\sigma(t, S)^2}{2}) dt + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class BlackScholesProcess : public GeneralizedBlackScholesProcess {
      public:
        BlackScholesProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

    //! Merton (1973) extension to the Black-Scholes stochastic process
    /*! This class describes the stochastic process ln(S) for a stock or
        stock index paying a continuous dividend yield given by
        \f[
            d\ln S(t, S) = (r(t) - q(t) - \frac{\sigma(t, S)^2}{2}) dt
                     + \sigma dW_t.
        \f]

        \ingroup processes
    */
    class BlackScholesMertonProcess : public GeneralizedBlackScholesProcess {
      public:
        BlackScholesMertonProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& dividendTS,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

    //! Black (1976) stochastic process
    /*! This class describes the stochastic process \f$ S \f$ for a
        forward or futures contract given by
        \f[
            d\ln S(t) = -\frac{\sigma(t, S)^2}{2} dt + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class BlackProcess : public GeneralizedBlackScholesProcess {
      public:
        BlackProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& riskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

    //! Garman-Kohlhagen (1983) stochastic process
    /*! This class describes the stochastic process \f$ S \f$ for an exchange
        rate given by
        \f[
            d\ln S(t) = (r(t) - r_f(t) - \frac{\sigma(t, S)^2}{2}) dt
                     + \sigma dW_t.
        \f]

        \warning while the interface is expressed in terms of \f$ S \f$,
                 the internal calculations work on \f$ ln S \f$.

        \ingroup processes
    */
    class GarmanKohlagenProcess : public GeneralizedBlackScholesProcess {
      public:
        GarmanKohlagenProcess(
            const Handle<Quote>& x0,
            const Handle<YieldTermStructure>& foreignRiskFreeTS,
            const Handle<YieldTermStructure>& domesticRiskFreeTS,
            const Handle<BlackVolTermStructure>& blackVolTS,
            const boost::shared_ptr<discretization>& d =
                  boost::shared_ptr<discretization>(new EulerDiscretization),
            bool forceDiscretization = false);
    };

}

/* -*- mode: c++; tab-width: 4; indent-tabs-mode: nil; c-basic-offset: 4 -*- */

/*
 Copyright (C) 2003 Ferdinando Ametrano
 Copyright (C) 2001, 2002, 2003 Sadruddin Rejeb
 Copyright (C) 2004, 2005, 2006, 2007 StatPro Italia srl
 Copyright (C) 2015 Peter Caspers

 This file is part of QuantLib, a free-software/open-source library
 for financial quantitative analysts and developers - http://quantlib.org/

 QuantLib is free software: you can redistribute it and/or modify it
 under the terms of the QuantLib license.  You should have received a
 copy of the license along with this program; if not, please email
 <quantlib-dev@lists.sf.net>. The license is also available online at
 <http://quantlib.org/license.shtml>.

 This program is distributed in the hope that it will be useful, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
 FOR A PARTICULAR PURPOSE.  See the license for more details.
*/

#include <ql/termstructures/volatility/equityfx/localvolsurface.hpp>
#include <ql/termstructures/volatility/equityfx/localvolcurve.hpp>
#include <ql/termstructures/volatility/equityfx/localconstantvol.hpp>
#include <ql/termstructures/yield/flatforward.hpp>
#include <ql/time/calendars/nullcalendar.hpp>
#include <ql/time/daycounters/actual365fixed.hpp>

#include <boost/make_shared.hpp>

namespace QuantLib {

    inline GeneralizedBlackScholesProcess::GeneralizedBlackScholesProcess(
             const Handle<Quote>& x0,
             const Handle<YieldTermStructure>& dividendTS,
             const Handle<YieldTermStructure>& riskFreeTS,
             const Handle<BlackVolTermStructure>& blackVolTS,
             const boost::shared_ptr<discretization>& disc,
             bool forceDiscretization)
    : StochasticProcess1D(disc), x0_(x0), riskFreeRate_(riskFreeTS),
      dividendYield_(dividendTS), blackVolatility_(blackVolTS),
      forceDiscretization_(forceDiscretization), updated_(false) {
        registerWith(x0_);
        registerWith(riskFreeRate_);
        registerWith(dividendYield_);
        registerWith(blackVolatility_);
    }

    inline Real GeneralizedBlackScholesProcess::x0() const {
        return x0_->value();
    }

    inline Real GeneralizedBlackScholesProcess::drift(Time t, Real x) const {
        Real sigma = diffusion(t,x);
        // we could be more anticipatory if we know the right dt
        // for which the drift will be used
        Time t1 = t + 0.0001;
        return riskFreeRate_->forwardRate(t,t1,Continuous,NoFrequency,true)
             - dividendYield_->forwardRate(t,t1,Continuous,NoFrequency,true)
             - 0.5 * sigma * sigma;
    }

    inline Real GeneralizedBlackScholesProcess::diffusion(Time t, Real x) const {
        return localVolatility()->localVol(t, x, true);
    }

    inline Real GeneralizedBlackScholesProcess::apply(Real x0, Real dx) const {
        return x0 * std::exp(dx);
    }

    inline Real GeneralizedBlackScholesProcess::expectation(Time t0,
                                                     Real x0,
                                                     Time dt) const {
        localVolatility(); // trigger update
        if(isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            return x0 *
                std::exp(dt * (riskFreeRate_->forwardRate(t0, t0 + dt, Continuous,
                                                          NoFrequency, true) -
                             dividendYield_->forwardRate(
                                 t0, t0 + dt, Continuous, NoFrequency, true)));
        } else {
            QL_FAIL("not implemented");
        }
    }

    inline Real GeneralizedBlackScholesProcess::stdDeviation(Time t0, Real x0, Time dt) const {
        localVolatility(); // trigger update
        if(isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            return std::sqrt(variance(t0,x0,dt));
        }
        else{
            return discretization_->diffusion(*this,t0,x0,dt);
        }
    }

    inline Real GeneralizedBlackScholesProcess::variance(Time t0, Real x0, Time dt) const {
        localVolatility(); // trigger update
        if(isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            return blackVolatility_->blackVariance(t0 + dt, 0.01) -
                   blackVolatility_->blackVariance(t0, 0.01);
        }
        else{
            return discretization_->variance(*this,t0,x0,dt);
        }
    }

    inline Real GeneralizedBlackScholesProcess::evolve(Time t0, Real x0,
                                                Time dt, Real dw) const {
        localVolatility(); // trigger update
        if (isStrikeIndependent_ && !forceDiscretization_) {
            // exact value for curves
            Real var = variance(t0, x0, dt);
            Real drift = (riskFreeRate_->forwardRate(t0, t0 + dt, Continuous,
                                                     NoFrequency, true) -
                          dividendYield_->forwardRate(t0, t0 + dt, Continuous,
                                                      NoFrequency, true)) *
                             dt -
                         0.5 * var;
            return apply(x0, std::sqrt(var) * dw + drift);
        } else
            return apply(x0, discretization_->drift(*this, t0, x0, dt) +
                                 stdDeviation(t0, x0, dt) * dw);
    }

    inline Time GeneralizedBlackScholesProcess::time(const Date& d) const {
        return riskFreeRate_->dayCounter().yearFraction(
                                           riskFreeRate_->referenceDate(), d);
    }

    inline void GeneralizedBlackScholesProcess::update() {
        updated_ = false;
        StochasticProcess1D::update();
    }

    inline const Handle<Quote>&
    GeneralizedBlackScholesProcess::stateVariable() const {
        return x0_;
    }

    inline const Handle<YieldTermStructure>&
    GeneralizedBlackScholesProcess::dividendYield() const {
        return dividendYield_;
    }

    inline const Handle<YieldTermStructure>&
    GeneralizedBlackScholesProcess::riskFreeRate() const {
        return riskFreeRate_;
    }

    inline const Handle<BlackVolTermStructure>&
    GeneralizedBlackScholesProcess::blackVolatility() const {
        return blackVolatility_;
    }

    inline const Handle<LocalVolTermStructure>&
    GeneralizedBlackScholesProcess::localVolatility() const {
        if (!updated_) {
            isStrikeIndependent_=true;

            // constant Black vol?
            boost::shared_ptr<BlackConstantVol> constVol =
                boost::dynamic_pointer_cast<BlackConstantVol>(
                                                          *blackVolatility());
            if (constVol) {
                // ok, the local vol is constant too.
                localVolatility_.linkTo(boost::make_shared<LocalConstantVol>(
                    constVol->referenceDate(),
                    constVol->blackVol(0.0, x0_->value()),
                    constVol->dayCounter()));
                updated_ = true;
                return localVolatility_;
            }

            // ok, so it's not constant. Maybe it's strike-independent?
            boost::shared_ptr<BlackVarianceCurve> volCurve =
                boost::dynamic_pointer_cast<BlackVarianceCurve>(
                                                          *blackVolatility());
            if (volCurve) {
                // ok, we can use the optimized algorithm
                localVolatility_.linkTo(boost::make_shared<LocalVolCurve>(
                    Handle<BlackVarianceCurve>(volCurve)));
                updated_ = true;
                return localVolatility_;
            }

            // ok, so it's strike-dependent. Never mind.
            localVolatility_.linkTo(
                boost::make_shared<LocalVolSurface>(blackVolatility_, riskFreeRate_,
                                                    dividendYield_, x0_->value()));
            updated_ = true;
            isStrikeIndependent_ = false;
            return localVolatility_;

        } else {
            return localVolatility_;
        }
    }

    // specific models

    inline BlackScholesProcess::BlackScholesProcess(
                              const Handle<Quote>& x0,
                              const Handle<YieldTermStructure>& riskFreeTS,
                              const Handle<BlackVolTermStructure>& blackVolTS,
                              const boost::shared_ptr<discretization>& d,
                              bool forceDiscretization)
    : GeneralizedBlackScholesProcess(
             x0,
             // no dividend yield
             Handle<YieldTermStructure>(boost::shared_ptr<YieldTermStructure>(
                  new FlatForward(0, NullCalendar(), 0.0, Actual365Fixed()))),
             riskFreeTS,
             blackVolTS,
             d,forceDiscretization) {}

    inline BlackScholesMertonProcess::BlackScholesMertonProcess(
                              const Handle<Quote>& x0,
                              const Handle<YieldTermStructure>& dividendTS,
                              const Handle<YieldTermStructure>& riskFreeTS,
                              const Handle<BlackVolTermStructure>& blackVolTS,
                              const boost::shared_ptr<discretization>& d,
                              bool forceDiscretization)
    : GeneralizedBlackScholesProcess(x0,dividendTS,riskFreeTS,blackVolTS,d,
                                     forceDiscretization) {}

    inline BlackProcess::BlackProcess(const Handle<Quote>& x0,
                               const Handle<YieldTermStructure>& riskFreeTS,
                               const Handle<BlackVolTermStructure>& blackVolTS,
                               const boost::shared_ptr<discretization>& d,
                               bool forceDiscretization)
    : GeneralizedBlackScholesProcess(x0,riskFreeTS,riskFreeTS,blackVolTS,d,
                                     forceDiscretization) {}

    inline GarmanKohlagenProcess::GarmanKohlagenProcess(
                          const Handle<Quote>& x0,
                          const Handle<YieldTermStructure>& foreignRiskFreeTS,
                          const Handle<YieldTermStructure>& domesticRiskFreeTS,
                          const Handle<BlackVolTermStructure>& blackVolTS,
                          const boost::shared_ptr<discretization>& d,
                          bool forceDiscretization)
    : GeneralizedBlackScholesProcess(x0,foreignRiskFreeTS,domesticRiskFreeTS,
                                     blackVolTS,d,forceDiscretization) {}

}

#endif


// Boost.Geometry

// Copyright (c) 2021, Oracle and/or its affiliates.

// Contributed and/or modified by Vissarion Fysikopoulos, on behalf of Oracle

// Licensed under the Boost Software License version 1.0.
// http://www.boost.org/users/license.html

#ifndef BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CLOSEST_POINTS_PT_SEG_HPP
#define BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CLOSEST_POINTS_PT_SEG_HPP

#include <boost/geometry/core/coordinate_promotion.hpp>

#include <boost/geometry/geometries/point.hpp>

#include <boost/geometry/strategies/cartesian/distance_pythagoras.hpp>
#include <boost/geometry/strategies/closest_points/services.hpp>

namespace boost { namespace geometry
{

namespace strategy { namespace closest_points
{

#ifndef DOXYGEN_NO_DETAIL
namespace detail
{

template <typename CalculationType>
struct compute_closest_point_to_segment
{
    template <typename Point, typename PointOfSegment>
    static inline auto
    apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2)
    {
        // A projected point of points in Integer coordinates must be able to be
        // represented in FP.
        using fp_point_type = model::point
            <
                CalculationType,
                dimension<PointOfSegment>::value,
                typename coordinate_system<PointOfSegment>::type
            >;

        // For convenience
        using fp_vector_type = fp_point_type;

        /*
            Algorithm [p: (px,py), p1: (x1,y1), p2: (x2,y2)]
            VECTOR v(x2 - x1, y2 - y1)
            VECTOR w(px - x1, py - y1)
            c1 = w . v
            c2 = v . v
            b = c1 / c2
            RETURN POINT(x1 + b * vx, y1 + b * vy)
        */

        // v is multiplied below with a (possibly) FP-value, so should be in FP
        // For consistency we define w also in FP
        fp_vector_type v, w, projected;

        geometry::convert(p2, v);
        geometry::convert(p, w);
        geometry::convert(p1, projected);
        subtract_point(v, projected);
        subtract_point(w, projected);

        CalculationType const zero = CalculationType();
        CalculationType const c1 = dot_product(w, v);
        if (c1 <= zero)
        {
            fp_vector_type fp_p1;
            geometry::convert(p1, fp_p1);
            return fp_p1;
        }
        CalculationType const c2 = dot_product(v, v);
        if (c2 <= c1)
        {
            fp_vector_type fp_p2;
            geometry::convert(p2, fp_p2);
            return fp_p2;
        }

        // See above, c1 > 0 AND c2 > c1 so: c2 != 0
        CalculationType const b = c1 / c2;

        multiply_value(v, b);
        add_point(projected, v);

        return projected;
    }
};

}
#endif // DOXYGEN_NO_DETAIL

template
<
    typename CalculationType = void
>
class projected_point
{
public:
    // The three typedefs below are necessary to calculate distances
    // from segments defined in integer coordinates.

    // Integer coordinates can still result in FP distances.
    // There is a division, which must be represented in FP.
    // So promote.

    template <typename Point, typename PointOfSegment>
    struct calculation_type
        : promote_floating_point
          <
            typename select_most_precise
                <
                    typename coordinate_type<Point>::type,
                    typename coordinate_type<PointOfSegment>::type,
                    CalculationType
                >::type
          >
    {};

    template <typename Point, typename PointOfSegment>
    inline auto
    apply(Point const& p, PointOfSegment const& p1, PointOfSegment const& p2) const
    {
        assert_dimension_equal<Point, PointOfSegment>();

        using calculation_type = typename calculation_type<Point, PointOfSegment>::type;

        return detail::compute_closest_point_to_segment<calculation_type>::apply(p, p1, p2);
    }

};

}} // namespace strategy::closest_points

}} // namespace boost::geometry

#endif // BOOST_GEOMETRY_STRATEGIES_CARTESIAN_CLOSEST_POINTS_PT_SEG_HPP