#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


#include <geometry.h>
#include <tiny_math_types.h>

#define BOOST_AUTO_TEST_MAIN
#include <boost/test/auto_unit_test.hpp>
#include <boost/test/unit_test_suite.hpp>
#include <boost/test/floating_point_comparison.hpp>
#include <boost/test/test_tools.hpp>

BOOST_AUTO_TEST_SUITE(geometry);

BOOST_AUTO_TEST_CASE(raycast_triangle)
{

  using std::sqrt;

  typedef tiny::MathTypes<double>   MT;
  typedef MT::vector3_type          V;
  typedef MT::real_type             T;
  typedef MT::value_traits          VT;

  V const p0 = V::make(0.0, 0.0, 0.0);
  V const p1 = V::make(1.0, 0.0, 0.0);
  V const p2 = V::make(0.0, 1.0, 0.0);

  geometry::Triangle<V> const triangle = geometry::make_triangle(p0,p1,p2);

  // Ray hitting from front
  {
    V                const p   = V::make( 0.0, 0.0, 1.0);
    V                const r   = V::make( 0.2, 0.2, -1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, true );

    BOOST_CHECK( hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }
  // Ray hitting from front
  {
    V                const p   = V::make( 0.0, 0.0, 1.0);
    V                const r   = V::make( 0.2, 0.2, -1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, false );

    BOOST_CHECK(  hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }
  // Ray hitting from back
  {
    V                const p   = V::make( 0.0, 0.0, -1.0);
    V                const r   = V::make( 0.2, 0.2,  1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, true );

    BOOST_CHECK( !hit );
  }
  // Ray hitting from back
  {
    V                const p   = V::make( 0.0, 0.0, -1.0);
    V                const r   = V::make( 0.2, 0.2,  1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, false );

    BOOST_CHECK(  hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(1),  0.2, 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }

  // Rays missing
  {
    V                const p   = V::make( 0.0, 0.0,  1.0);
    V                const r   = V::make( 0.6, 0.6, -1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, false );

    BOOST_CHECK(  !hit );
  }
  {
    V                const p   = V::make( 0.0, 0.0,  1.0);
    V                const r   = V::make( 0.5,-0.1, -1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, false );

    BOOST_CHECK(  !hit );
  }

  // Ray gracing
  {
    V                const p   = V::make( 0.0, 0.0, -1.0);
    V                const r   = V::make( 0.4999, 0.4999,  1.0);
    geometry::Ray<V> const ray = geometry::make_ray(p, r);

    T       length = VT::zero();
    V       q      = V::zero();

    bool hit = geometry::compute_raycast_triangle(ray, triangle, q, length, false );

    BOOST_CHECK(  hit );
    BOOST_CHECK_CLOSE( length, tiny::norm(r), 0.01);
    BOOST_CHECK_CLOSE( q(0),  r(0), 0.01);
    BOOST_CHECK_CLOSE( q(1),  r(1), 0.01);
    BOOST_CHECK_CLOSE( q(2),  0.0, 0.01);
  }

}

BOOST_AUTO_TEST_SUITE_END();


//==================================================================================================
/*!
  @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


//  (C) Copyright Matt Borland 2022.
//  Use, modification and distribution are subject to 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/math/tools/color_maps.hpp>
#include "test_compile_result.hpp"

void compile_and_link_test()
{
    check_result<float>(boost::math::tools::black_body<float>(0.5f)[0]);
    check_result<float>(boost::math::tools::extended_kindlmann<float>(0.5f)[0]);
    check_result<double>(boost::math::tools::inferno<double>(0.5)[0]);
    check_result<double>(boost::math::tools::kindlmann<double>(0.5)[0]);
    check_result<float>(boost::math::tools::plasma<float>(0.5f)[0]);
    check_result<float>(boost::math::tools::smooth_cool_warm<float>(0.5f)[0]);
    check_result<float>(boost::math::tools::viridis<float>(0.5f)[0]);
}


#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);
}


#define CATCH_CONFIG_MAIN

#include "CALPHADFreeEnergyFunctionsBinary.h"
#include "InterpolationType.h"

#include "catch.hpp"

#include <boost/optional/optional.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <fstream>
#include <iostream>
#include <string>

namespace pt = boost::property_tree;

TEST_CASE("CALPHAD binary KKS", "[binary kks]")
{
    Thermo4PFM::EnergyInterpolationType energy_interp_func_type
        = Thermo4PFM::EnergyInterpolationType::PBG;
    Thermo4PFM::ConcInterpolationType conc_interp_func_type
        = Thermo4PFM::ConcInterpolationType::PBG;

    double temperature = 1450.;

    std::cout << " Read CALPHAD database..." << std::endl;
    pt::ptree calphad_db;
    try
    {
        pt::read_json("../thermodynamic_data/calphadAuNi.json", calphad_db);
    }
    catch (std::exception& e)
    {
        std::cerr << "exception caught: " << e.what() << std::endl;
    }

    boost::optional<pt::ptree&> newton_db;

    Thermo4PFM::CALPHADFreeEnergyFunctionsBinary cafe(
        calphad_db, newton_db, energy_interp_func_type, conc_interp_func_type);

    // initial guesses
    double c_init0 = 0.5;
    double c_init1 = 0.5;

    double sol[2] = { c_init0, c_init1 };

    // compute concentrations satisfying KKS equations
    double conc = 0.3;
    double phi  = 0.5;
    cafe.computePhaseConcentrations(temperature, &conc, &phi, &sol[0]);

    std::cout << "-------------------------------" << std::endl;
    std::cout << "Temperature = " << temperature << std::endl;
    std::cout << "Result for c = " << conc << " and phi = " << phi << std::endl;
    std::cout << "   cL = " << sol[0] << std::endl;
    std::cout << "   cS = " << sol[1] << std::endl;

    const Thermo4PFM::PhaseIndex pi0 = Thermo4PFM::PhaseIndex::phaseL;
    const Thermo4PFM::PhaseIndex pi1 = Thermo4PFM::PhaseIndex::phaseA;

    std::cout << "Verification:" << std::endl;

    double derivL;
    cafe.computeDerivFreeEnergy(temperature, &sol[0], pi0, &derivL);
    std::cout << "   dfL/dcL = " << derivL << std::endl;

    double derivS;
    cafe.computeDerivFreeEnergy(temperature, &sol[1], pi1, &derivS);
    std::cout << "   dfS/dcS = " << derivS << std::endl;

    REQUIRE(derivS == Approx(derivL).margin(1.e-5));
}


/* boost random/lognormal_distribution.hpp header file
 *
 * Copyright Jens Maurer 2000-2001
 * Permission to use, copy, modify, sell, and distribute this software
 * is hereby granted without fee provided that the above copyright notice
 * appears in all copies and that both that copyright notice and this
 * permission notice appear in supporting documentation,
 *
 * Jens Maurer makes no representations about the suitability of this
 * software for any purpose. It is provided "as is" without express or
 * implied warranty.
 *
 * See http://www.boost.org for most recent version including documentation.
 *
 * $Id: lognormal_distribution.hpp,v 1.13 2003/02/25 10:29:29 bjorn_karlsson Exp $
 *
 * Revision history
 *  2001-02-18  moved to individual header files
 */

#ifndef BOOST_RANDOM_LOGNORMAL_DISTRIBUTION_HPP
#define BOOST_RANDOM_LOGNORMAL_DISTRIBUTION_HPP

#include <cmath>      // std::exp, std::sqrt
#include <cassert>
#include <boost/random/normal_distribution.hpp>

#ifdef BOOST_NO_STDC_NAMESPACE
namespace std {
  using ::log;
  using ::sqrt;
}
#endif

namespace boost {

#if defined(__GNUC__) && (__GNUC__ < 3)
// Special gcc workaround: gcc 2.95.x ignores using-declarations
// in template classes (confirmed by gcc author Martin v. Loewis)
  using std::sqrt;
  using std::exp;
#endif

template<class UniformRandomNumberGenerator, class RealType = double,
        class Adaptor = uniform_01<UniformRandomNumberGenerator, RealType> >
class lognormal_distribution
{
public:
  typedef Adaptor adaptor_type;
  typedef UniformRandomNumberGenerator base_type;
  typedef RealType result_type;

  explicit lognormal_distribution(base_type & rng,
                                  result_type mean = result_type(1),
                                  result_type sigma = result_type(1))
    : _mean(mean), _sigma(sigma),
      _rng(rng, std::log(mean*mean/std::sqrt(sigma*sigma + mean*mean)),
           std::sqrt(std::log(sigma*sigma/mean/mean+result_type(1))))
  {
    assert(mean > result_type(0));
  }

  // compiler-generated copy ctor and assignment operator are fine

  adaptor_type& adaptor() { return _rng.adaptor(); }
  base_type& base() const { return _rng.base(); }
  RealType mean() const { return _mean; }
  RealType sigma() const { return _sigma; }
  void reset() { _rng.reset(); }

  result_type operator()()
  {
#ifndef BOOST_NO_STDC_NAMESPACE
    // allow for Koenig lookup
    using std::exp;
#endif
    return exp(_rng());
  }

#ifndef BOOST_NO_OPERATORS_IN_NAMESPACE
  friend bool operator==(const lognormal_distribution& x,
                         const lognormal_distribution& y)
  { return x._rng == y._rng; }

#ifndef BOOST_NO_MEMBER_TEMPLATE_FRIENDS
  template<class CharT, class Traits>
  friend std::basic_ostream<CharT,Traits>&
  operator<<(std::basic_ostream<CharT,Traits>& os, const lognormal_distribution& ld)
  {
    os << ld._rng << " " << ld._mean << " " << ld._sigma;
    return os;
  }

  template<class CharT, class Traits>
  friend std::basic_istream<CharT,Traits>&
  operator>>(std::basic_istream<CharT,Traits>& is, lognormal_distribution& ld)
  {
    is >> std::ws >> ld._rng >> std::ws >> ld._mean >> std::ws >> ld._sigma;
    return is;
  }
#endif

#else
  // Use a member function
  bool operator==(const lognormal_distribution& rhs) const
  { return _rng == rhs._rng;  }
#endif
private:
  RealType _mean, _sigma;
  normal_distribution<base_type, result_type, adaptor_type> _rng;
};

} // namespace boost

#endif // BOOST_RANDOM_LOGNORMAL_DISTRIBUTION_HPP


// Boost.Units - A C++ library for zero-overhead dimensional analysis and
// unit/quantity manipulation and conversion
//
// Copyright (C) 2009 Steven Watanabe
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)

/**
\file

\brief test_trig.cpp

\details
Test trigonometric functions.

Output:
@verbatim
@endverbatim
**/

#include <cmath>
#include <boost/units/cmath.hpp>
#include <boost/units/io.hpp>
#include <boost/units/systems/si/plane_angle.hpp>
#include <boost/units/systems/si/length.hpp>
#include <boost/units/systems/si/dimensionless.hpp>
#include <boost/units/systems/angle/degrees.hpp>

#include "test_close.hpp"

using boost::units::si::radians;
using boost::units::si::si_dimensionless;
using boost::units::degree::degrees;
BOOST_UNITS_STATIC_CONSTANT(degree_dimensionless, boost::units::degree::dimensionless);
using boost::units::si::meters;
BOOST_UNITS_STATIC_CONSTANT(heterogeneous_dimensionless, boost::units::reduce_unit<boost::units::si::dimensionless>::type);

void test_sin() {
    BOOST_TEST_EQ(boost::units::sin(2.0 * radians), std::sin(2.0) * si_dimensionless);
    BOOST_UNITS_TEST_CLOSE(static_cast<double>(boost::units::sin(15.0 * degrees)), 0.2588, 0.0004);
}

void test_cos() {
    BOOST_TEST_EQ(boost::units::cos(2.0 * radians), std::cos(2.0) * si_dimensionless);
    BOOST_UNITS_TEST_CLOSE(static_cast<double>(boost::units::cos(75.0 * degrees)), 0.2588, 0.0004);
}

void test_tan() {
    BOOST_TEST_EQ(boost::units::tan(2.0 * radians), std::tan(2.0) * si_dimensionless);
    BOOST_UNITS_TEST_CLOSE(static_cast<double>(boost::units::tan(45.0 * degrees)), 1.0, 0.0001);
}

void test_asin() {
    BOOST_TEST_EQ(boost::units::asin(0.2 * si_dimensionless), std::asin(0.2) * radians);
    BOOST_UNITS_TEST_CLOSE(boost::units::asin(0.5 * degree_dimensionless).value(), 30.0, 0.0001);
    BOOST_TEST_EQ(boost::units::asin(0.2 * heterogeneous_dimensionless).value(), std::asin(0.2));
}

void test_acos() {
    BOOST_TEST_EQ(boost::units::acos(0.2 * si_dimensionless), std::acos(0.2) * radians);
    BOOST_UNITS_TEST_CLOSE(boost::units::acos(0.5 * degree_dimensionless).value(), 60.0, 0.0001);
    BOOST_TEST_EQ(boost::units::acos(0.2 * heterogeneous_dimensionless).value(), std::acos(0.2));
}

void test_atan() {
    BOOST_TEST_EQ(boost::units::atan(0.2 * si_dimensionless), std::atan(0.2) * radians);
    BOOST_UNITS_TEST_CLOSE(boost::units::atan(1.0 * degree_dimensionless).value(), 45.0, 0.0001);
    BOOST_TEST_EQ(boost::units::atan(0.2 * heterogeneous_dimensionless).value(), std::atan(0.2));
}

void test_atan2() {
    BOOST_TEST_EQ(boost::units::atan2(0.2 * si_dimensionless, 0.3 * si_dimensionless), std::atan2(0.2, 0.3) * radians);
    BOOST_TEST_EQ(boost::units::atan2(0.2 * meters, 0.3 * meters), std::atan2(0.2, 0.3) * radians);
    BOOST_UNITS_TEST_CLOSE(boost::units::atan2(0.8660*degree_dimensionless,0.5*degree_dimensionless).value(), 60., 0.0002);
    BOOST_TEST_EQ(boost::units::atan2(0.2 * heterogeneous_dimensionless, 0.3 * heterogeneous_dimensionless).value(), std::atan2(0.2, 0.3));
}

int main()
{
    test_sin();
    test_cos();
    test_tan();
    test_asin();
    test_acos();
    test_atan();
    test_atan2();
    return boost::report_errors();
}


#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;
}