/*
 * Licensed to the Apache Software Foundation (ASF) under one
 * or more contributor license agreements.  See the NOTICE file
 * distributed with this work for additional information
 * regarding copyright ownership.  The ASF licenses this file
 * to you 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.
 */
/* ------------------------------------------------------
 *
 * @file logistic.cpp
 *
 * @brief Logistic-Regression functions
 *
 * We implement the conjugate-gradient method and the iteratively-reweighted-
 * least-squares method.
 *
 *//* ----------------------------------------------------------------------- */
#include <limits>
#include <dbconnector/dbconnector.hpp>
#include <modules/shared/HandleTraits.hpp>
#include <modules/prob/boost.hpp>
#include <boost/math/distributions.hpp>
#include <modules/prob/student.hpp>
#include "simple_logistic.hpp"

namespace madlib {

// Use Eigen
using namespace dbal::eigen_integration;

namespace modules {

// Import names from other MADlib modules
using dbal::NoSolutionFoundException;

namespace hello_world {

// FIXME this enum should be accessed by all modules that may need grouping
// valid status values
enum { IN_PROCESS, COMPLETED, TERMINATED, NULL_EMPTY };

// Internal functions
AnyType stateToResult(const Allocator &inAllocator,
                      const HandleMap<const ColumnVector, TransparentHandle<double> >& inCoef,
                      const Matrix & hessian,
                      const double &logLikelihood,
                      int status,
                      const uint64_t &numRows);

// ---------------------------------------------------------------------------
//              Logistic Regression States
// ---------------------------------------------------------------------------

/**
 * @brief Inter- and intra-iteration state for conjugate-gradient method for
 *        logistic regression
 *
 * TransitionState encapsualtes the transition state during the
 * logistic-regression aggregate function. To the database, the state is
 * exposed as a single DOUBLE PRECISION array, to the C++ code it is a proper
 * object containing scalars and vectors.
 *
 * Note: We assume that the DOUBLE PRECISION array is initialized by the
 * database with length at least 5, and all elemenets are 0.
 *
 */
template <class Handle>
class LogRegrSPTransitionState {
    template <class OtherHandle>
    friend class LogRegrSPTransitionState;

  public:
    LogRegrSPTransitionState(const AnyType &inArray)
        : mStorage(inArray.getAs<Handle>()) {

        rebind(static_cast<uint16_t>(mStorage[1]));
    }

    /**
     * @brief Convert to backend representation
     *
     * We define this function so that we can use State in the
     * argument list and as a return type.
     */
    inline operator AnyType() const {
        return mStorage;
    }

    /**
     * @brief Initialize the conjugate-gradient state.
     *
     * This function is only called for the first iteration, for the first row.
     */
    inline void initialize(const Allocator &inAllocator, uint16_t inWidthOfX) {
        mStorage = inAllocator.allocateArray<double, dbal::AggregateContext,
                                             dbal::DoZero, dbal::ThrowBadAlloc>(arraySize(inWidthOfX));
        rebind(inWidthOfX);
        widthOfX = inWidthOfX;
    }

    /**
     * @brief We need to support assigning the previous state
     */
    template <class OtherHandle>
    LogRegrSPTransitionState &operator=(
        const LogRegrSPTransitionState<OtherHandle> &inOtherState) {

        for (size_t i = 0; i < mStorage.size(); i++)
            mStorage[i] = inOtherState.mStorage[i];
        return *this;
    }

    /**
     * @brief Merge with another State object by copying the intra-iteration
     *     fields
     */
    template <class OtherHandle>
    LogRegrSPTransitionState &operator+=(
        const LogRegrSPTransitionState<OtherHandle> &inOtherState) {

        if (mStorage.size() != inOtherState.mStorage.size() ||
            widthOfX != inOtherState.widthOfX)
            throw std::logic_error("Internal error: Incompatible transition "
                                   "states");

        numRows += inOtherState.numRows;
        gradNew += inOtherState.gradNew;
        X_transp_AX += inOtherState.X_transp_AX;
        logLikelihood += inOtherState.logLikelihood;
        // merged state should have the higher status
        // (see top of file for more on 'status' )
        status = (inOtherState.status > status) ? inOtherState.status : status;
        return *this;
    }

    /**
     * @brief Reset the inter-iteration fields.
     */
    inline void reset() {
        numRows = 0;
        X_transp_AX.fill(0);
        gradNew.fill(0);
        logLikelihood = 0;
        status = IN_PROCESS;
    }

  private:
    static inline size_t arraySize(const uint16_t inWidthOfX) {
        return 6 + inWidthOfX * inWidthOfX + 4 * inWidthOfX;
    }

    /**
     * @brief Rebind to a new storage array
     *
     * @param inWidthOfX The number of independent variables.
     *
     * Array layout (iteration refers to one aggregate-function call):
     * Inter-iteration components (updated in final function):
     * - 0: iteration (current iteration)
     * - 1: widthOfX (number of coefficients)
     * - 2: coef (vector of coefficients)
     * - 2 + widthOfX: dir (direction)
     * - 2 + 2 * widthOfX: grad (gradient)
     * - 2 + 3 * widthOfX: beta (scale factor)
     *
     * Intra-iteration components (updated in transition step):
     * - 3 + 3 * widthOfX: numRows (number of rows already processed in this iteration)
     * - 4 + 3 * widthOfX: gradNew (intermediate value for gradient)
     * - 4 + 4 * widthOfX: X_transp_AX (X^T A X)
     * - 4 + widthOfX * widthOfX + 4 * widthOfX: logLikelihood ( ln(l(c)) )
     */
    void rebind(uint16_t inWidthOfX) {
        iteration.rebind(&mStorage[0]);
        widthOfX.rebind(&mStorage[1]);
        coef.rebind(&mStorage[2], inWidthOfX);
        dir.rebind(&mStorage[2 + inWidthOfX], inWidthOfX);
        grad.rebind(&mStorage[2 + 2 * inWidthOfX], inWidthOfX);
        beta.rebind(&mStorage[2 + 3 * inWidthOfX]);
        numRows.rebind(&mStorage[3 + 3 * inWidthOfX]);
        gradNew.rebind(&mStorage[4 + 3 * inWidthOfX], inWidthOfX);
        X_transp_AX.rebind(&mStorage[4 + 4 * inWidthOfX], inWidthOfX, inWidthOfX);
        logLikelihood.rebind(&mStorage[4 + inWidthOfX * inWidthOfX + 4 * inWidthOfX]);
        status.rebind(&mStorage[5 + inWidthOfX * inWidthOfX + 4 * inWidthOfX]);
    }

    Handle mStorage;

  public:
    typename HandleTraits<Handle>::ReferenceToUInt32 iteration;
    typename HandleTraits<Handle>::ReferenceToUInt16 widthOfX;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap coef;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap dir;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap grad;
    typename HandleTraits<Handle>::ReferenceToDouble beta;

    typename HandleTraits<Handle>::ReferenceToUInt64 numRows;
    typename HandleTraits<Handle>::ColumnVectorTransparentHandleMap gradNew;
    typename HandleTraits<Handle>::MatrixTransparentHandleMap X_transp_AX;
    typename HandleTraits<Handle>::ReferenceToDouble logLikelihood;
    typename HandleTraits<Handle>::ReferenceToUInt16 status;
};

/**
 * @brief Logistic function
 */
inline double sigma(double x) {
    return 1. / (1. + std::exp(-x));
}

/**
 * @brief Perform the logistic-regression transition step
 */
AnyType
logregr_simple_step_transition::run(AnyType &args) {
    LogRegrSPTransitionState<MutableArrayHandle<double> > state = args[0];
    if (args[1].isNull() || args[2].isNull()) { return args[0]; }
    double y = args[1].getAs<bool>() ? 1. : -1.;
    MappedColumnVector x;
    try {
        // an exception is raised in the backend if args[2] contains nulls
        MappedColumnVector xx = args[2].getAs<MappedColumnVector>();
        // x is a const reference, we can only rebind to change its pointer
        x.rebind(xx.memoryHandle(), xx.size());
    } catch (const ArrayWithNullException &e) {
        return args[0];
    }

    // The following check was added with MADLIB-138.
    if (!dbal::eigen_integration::isfinite(x)) {
        warning("Design matrix is not finite.");
        state.status = TERMINATED;
        return state;
    }

    if (state.numRows == 0) {
        if (x.size() > std::numeric_limits<uint16_t>::max()){
            warning("Number of independent variables cannot be larger than 65535.");
            state.status = TERMINATED;
            return state;
        }

        state.initialize(*this, static_cast<uint16_t>(x.size()));
        if (!args[3].isNull()) {
            LogRegrSPTransitionState<ArrayHandle<double> > previousState = args[3];

            state = previousState;
            state.reset();
        }
    }
    // Now do the transition step
    state.numRows++;
    double xc = dot(x, state.coef);
    state.gradNew.noalias() += sigma(-y * xc) * y * trans(x);

    // Note: sigma(-x) = 1 - sigma(x).
    double a = sigma(xc) * sigma(-xc);
    state.X_transp_AX += x * trans(x) * a;

    state.logLikelihood -= std::log( 1. + std::exp(-y * xc) );

    return state;
}

/**
 * @brief Perform the perliminary aggregation function: Merge transition states
 */
AnyType
logregr_simple_step_merge_states::run(AnyType &args) {
    LogRegrSPTransitionState<MutableArrayHandle<double> > stateLeft = args[0];
    LogRegrSPTransitionState<ArrayHandle<double> > stateRight = args[1];

    // We first handle the trivial case where this function is called with one
    // of the states being the initial state
    if (stateLeft.numRows == 0)
        return stateRight;
    else if (stateRight.numRows == 0)
        return stateLeft;

    // Merge states together and return
    stateLeft += stateRight;
    return stateLeft;
}

/**
 * @brief Perform the logistic-regression final step
 */
AnyType
logregr_simple_step_final::run(AnyType &args) {
    // We request a mutable object. Depending on the backend, this might perform
    // a deep copy.
    LogRegrSPTransitionState<MutableArrayHandle<double> > state = args[0];

    // Aggregates that haven't seen any data just return Null.
    if (state.numRows == 0){
        state.status = NULL_EMPTY;
        return state;
    }

    // Note: k = state.iteration
    if (state.iteration == 0) {
        // Iteration computes the gradient

        state.dir = state.gradNew;
        state.grad = state.gradNew;
    } else {
        // We use the Hestenes-Stiefel update formula:
        //
        //            g_k^T (g_k - g_{k-1})
        // beta_k = -------------------------
        //          d_{k-1}^T (g_k - g_{k-1})
        ColumnVector gradNewMinusGrad = state.gradNew - state.grad;
        state.beta
            = dot(state.gradNew, gradNewMinusGrad)
            / dot(state.dir, gradNewMinusGrad);

        // Do a direction restart (Powell restart)
        // Note: This is testing whether state.beta < 0 if state.beta were
        // assigned according to Polak-Ribière
        if (dot(state.gradNew, gradNewMinusGrad)
            / dot(state.grad, state.grad) <= std::numeric_limits<double>::denorm_min()) state.beta = 0;

        // d_k = g_k - beta_k * d_{k-1}
        state.dir = state.gradNew - state.beta * state.dir;
        state.grad = state.gradNew;
    }

    // H_k = - X^T A_k X
    // where A_k = diag(a_1, ..., a_n) and a_i = sigma(x_i c_{k-1}) sigma(-x_i c_{k-1})
    //
    //             g_k^T d_k
    // alpha_k = -------------
    //           d_k^T H_k d_k
    //
    // c_k = c_{k-1} - alpha_k * d_k
    state.coef += dot(state.grad, state.dir) /
        as_scalar(trans(state.dir) * state.X_transp_AX * state.dir)
        * state.dir;

    if(!state.coef.is_finite()){
        // we don't throw an error - instead set the state status to allow
        // other states (possibly trained as part of group by) to continue
        // training
        warning("Over- or underflow in conjugate-gradient step, while updating "
              "coefficients. Input data is likely of poor numerical condition.");
        state.status = TERMINATED;
        return state;
    }

    state.iteration++;
    return state;
}

/**
 * @brief Return the difference in log-likelihood between two states
 */
AnyType
internal_logregr_simple_step_distance::run(AnyType &args) {
    LogRegrSPTransitionState<ArrayHandle<double> > stateLeft = args[0];
    LogRegrSPTransitionState<ArrayHandle<double> > stateRight = args[1];

    if(stateLeft.status == NULL_EMPTY || stateRight.status == NULL_EMPTY){
        return 0.0;
    }
    return std::abs(stateLeft.logLikelihood - stateRight.logLikelihood);
}

/**
 * @brief Return the coefficients and diagnostic statistics of the state
 */
AnyType
internal_logregr_simple_result::run(AnyType &args) {
    LogRegrSPTransitionState<ArrayHandle<double> > state = args[0];
    if (state.status == NULL_EMPTY)
        return Null();

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        state.X_transp_AX, EigenvaluesOnly, ComputePseudoInverse);

    return stateToResult(*this, state.coef,
                         state.X_transp_AX, state.logLikelihood,
                         state.status, state.numRows);
}

/**
 * @brief Compute the diagnostic statistics
 *
 * This function wraps the common parts of computing the results for both the
 * CG and the IRLS method.
 */
AnyType stateToResult(
    const Allocator &inAllocator,
    const HandleMap<const ColumnVector, TransparentHandle<double> > &inCoef,
    const Matrix & hessian,
    const double &logLikelihood,
    int status,
    const uint64_t &numRows) {

    SymmetricPositiveDefiniteEigenDecomposition<Matrix> decomposition(
        hessian, EigenvaluesOnly, ComputePseudoInverse);

    const Matrix &inverse_of_X_transp_AX = decomposition.pseudoInverse();
    const ColumnVector &diagonal_of_X_transp_AX = inverse_of_X_transp_AX.diagonal();

    MutableNativeColumnVector stdErr(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldZStats(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector waldPValues(
        inAllocator.allocateArray<double>(inCoef.size()));
    MutableNativeColumnVector oddsRatios(
        inAllocator.allocateArray<double>(inCoef.size()));

    for (Index i = 0; i < inCoef.size(); ++i) {
        stdErr(i) = std::sqrt(diagonal_of_X_transp_AX(i));
        waldZStats(i) = inCoef(i) / stdErr(i);
        waldPValues(i) = 2. * prob::cdf( prob::normal(),
                                         -std::abs(waldZStats(i)));
        oddsRatios(i) = std::exp( inCoef(i) );
    }

    // Return all coefficients, standard errors, etc. in a tuple
    AnyType tuple;
    tuple << inCoef << logLikelihood << stdErr << waldZStats << waldPValues
          << oddsRatios << inverse_of_X_transp_AX
          << sqrt(decomposition.conditionNo()) << status << numRows;
    return tuple;
}

} // namespace hello_world
} // namespace modules
} // namespace madlib


//==================================================================================================
/*!
  @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_LOG2_HPP_INCLUDED
#define BOOST_SIMD_ARCH_COMMON_SCALAR_FUNCTION_LOG2_HPP_INCLUDED
#include <boost/simd/function/std.hpp>

#include <boost/simd/detail/dispatch/function/overload.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/std.hpp>
#include <boost/assert.hpp>
#include <boost/config.hpp>
#include <cmath>
#include <boost/simd/function/bitwise_and.hpp>
#include <boost/simd/function/dec.hpp>
#include <boost/simd/function/fma.hpp>
#include <boost/simd/function/frexp.hpp>
#include <boost/simd/function/horn.hpp>
#include <boost/simd/function/ilog2.hpp>
#include <boost/simd/function/musl.hpp>
#include <boost/simd/function/plain.hpp>
#include <boost/simd/function/sqr.hpp>
#include <boost/simd/function/std.hpp>

#include <boost/simd/constant/inf.hpp>
#include <boost/simd/constant/invlog_2.hpp>
#include <boost/simd/constant/nan.hpp>
#include <boost/simd/constant/minf.hpp>
#include <boost/simd/constant/smallestposval.hpp>
#include <boost/simd/constant/sqrt_2o_2.hpp>

#include <boost/simd/detail/constant/invlog_2hi.hpp>
#include <boost/simd/detail/constant/invlog_2lo.hpp>

#include <boost/simd/detail/dispatch/meta/as_integer.hpp>

#include <cmath>
namespace boost { namespace simd { namespace ext
{
  namespace bd = boost::dispatch;
  namespace bs = boost::simd;

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return musl_(log2)(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (A0 const& a0) const BOOST_NOEXCEPT
    {
      return bs::ilog2(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::std_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return std::log2(a0);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::arithmetic_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const std_tag &, A0 a0
                                    ) const BOOST_NOEXCEPT
    {
      return musl_(log2(a0));
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::single_<A0> >
                          )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2f.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunPro, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
     BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 ix = bitwise_cast<uiA0>(x);
      iA0 k = 0;
      if (ix < 0x00800000 || ix>>31)         /* x < 2**-126  */
      {
        if (ix<<1 == 0) return Minf<A0>();  /* log(+-0)=-inf */
        if (ix>>31) return Nan<A0>();       /* log(-#) = NaN */
#ifndef BOOST_SIMD_NO_DENORMALS
        /* subnormal number, scale up x */
        k -= 25;
        x *= 33554432.0f;
         ix = bitwise_cast<iA0>(x);
#endif
      }
      else if (ix >= 0x7f800000)
      {
        return x;
      }
      else if (ix == 0x3f800000)
        return 0;

      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      ix += 0x3f800000 - 0x3f3504f3;
      k += bitwise_cast<iA0>(ix>>23) - 0x7f;
      ix = (ix&0x007fffff) + 0x3f3504f3;
      x =  bitwise_cast<A0>(ix);
      A0 f = dec(x);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3eccce13, 0x3e789e26>(w);
      A0 t2= z*horn<A0, 0x3f2aaaaa, 0x3e91e9ee>(w);
      A0 R = t2 + t1;
      A0 hfsq = 0.5f*sqr(f);
      return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+k;
      // The original algorithm does some extra calculation in place of the return line
      // to get extra precision but this is uneeded for float as the exhaustive test shows
      // a 0.5 ulp maximal error on the full range.
      // Moreover all log2(exp2(i)) i =  1..31 are flint
      // I leave the code here in case an exotic proc will not play the game.
      //       A0  hi = f - hfsq;
      //       hi =  bitwise_and(hi, uiA0(0xfffff000ul));
      //       A0  lo = fma(s, hfsq+R, f - hi - hfsq);
      //       return fma((lo+hi), Invlog_2lo<A0>(), lo*Invlog_2hi<A0>() + hi*Invlog_2hi<A0>() + k);
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::musl_tag
                          , bd::scalar_< bd::double_<A0> >
                          )
  {
    /* origin: FreeBSD /usr/src/lib/msun/src/e_log2.c */
    /*
     * ====================================================
     * Copyright (C) 1993 by Sun Microsystems, Inc. All rights reserved.
     *
     * Developed at SunSoft, a Sun Microsystems, Inc. business.
     * Permission to use, copy, modify, and distribute this
     * software is freely granted, provided that this notice
     * is preserved.
     * ====================================================
     */
    BOOST_FORCEINLINE A0 operator() (const musl_tag &, A0 x) const BOOST_NOEXCEPT
    {
      using uiA0 = bd::as_integer_t<A0, unsigned>;
      using iA0 = bd::as_integer_t<A0,   signed>;
      uiA0 hx = bitwise_cast<uiA0>(x) >> 32;
      iA0 k = 0;
      if (hx < 0x00100000 || hx>>31)
      {
        if(is_eqz(x))
          return Minf<A0>();  /* log(+-0)=-inf */
        if (hx>>31)
          return Nan<A0>(); /* log(-#) = NaN */
        /* subnormal number, scale x up */
#ifndef BOOST_SIMD_NO_DENORMALS
        k -= 54;
        x *=  18014398509481984.0;
        hx = bitwise_cast<uiA0>(x) >> 32;
#endif
      }
      else if (hx >= 0x7ff00000)
      {
        return x;
      }
      else if (x == One<A0>())
        return Zero<A0>();
      /* reduce x into [sqrt(2)/2, sqrt(2)] */
      hx += 0x3ff00000 - 0x3fe6a09e;
      k += bitwise_cast<iA0>(hx>>20) - 0x3ff;
      hx = (hx&0x000fffff) + 0x3fe6a09e;
      x = bitwise_cast<A0>( (uint64_t)hx<<32 | (bitwise_and(0xffffffffull, bitwise_cast<uiA0>(x))));
      A0 f = dec(x);
      A0 hfsq = 0.5*sqr(f);
      A0 s = f/(2.0f + f);
      A0 z = sqr(s);
      A0 w = sqr(z);
      A0 t1= w*horn<A0, 0x3fd999999997fa04ll, 0x3fcc71c51d8e78afll, 0x3fc39a09d078c69fll > (w);
      A0 t2= z*horn<A0, 0x3fe5555555555593ll, 0x3fd2492494229359ll
                      , 0x3fc7466496cb03dell, 0x3fc2f112df3e5244ll> (w);
      A0 R = t2 + t1;
//        return -(hfsq-(s*(hfsq+R))-f)*Invlog_2<A0>()+dk;  // fast ?

      /*
       * f-hfsq must (for args near 1) be evaluated in extra precision
       * to avoid a large cancellation when x is near sqrt(2) or 1/sqrt(2).
       * This is fairly efficient since f-hfsq only depends on f, so can
       * be evaluated in parallel with R.  Not combining hfsq with R also
       * keeps R small (though not as small as a true `lo' term would be),
       * so that extra precision is not needed for terms involving R.
       *
       * Compiler bugs involving extra precision used to break Dekker's
       * theorem for spitting f-hfsq as hi+lo, unless double_t was used
       * or the multi-precision calculations were avoided when double_t
       * has extra precision.  These problems are now automatically
       * avoided as a side effect of the optimization of combining the
       * Dekker splitting step with the clear-low-bits step.
       *
       * y must (for args near sqrt(2) and 1/sqrt(2)) be added in extra
       * precision to avoid a very large cancellation when x is very near
       * these values.  Unlike the above cancellations, this problem is
       * specific to base 2.  It is strange that adding +-1 is so much
       * harder than adding +-ln2 or +-log10_2.
       *
       * This uses Dekker's theorem to normalize y+val_hi, so the
       * compiler bugs are back in some configurations, sigh.  And I
       * don't want to used double_t to avoid them, since that gives a
       * pessimization and the support for avoiding the pessimization
       * is not yet available.
       *
       * The multi-precision calculations for the multiplications are
       * routine.
       */

      /* hi+lo = f - hfsq + s*(hfsq+R) ~ log(1+f) */
      A0  hi = f - hfsq;
      hi =  bitwise_and(hi, (Allbits<uiA0>() << 32));
      A0 lo = f - hi - hfsq + s*(hfsq+R);

      A0 val_hi = hi*Invlog_2hi<A0>();
      A0 val_lo = fma(lo+hi, Invlog_2lo<A0>(), lo*Invlog_2hi<A0>());

      A0 dk = k;
      A0 w1 = dk + val_hi;
      val_lo += (dk - w1) + val_hi;
      val_hi = w1;
      return val_lo + val_hi;
    }
  };

  BOOST_DISPATCH_OVERLOAD ( log2_
                          , (typename A0)
                          , bd::cpu_
                          , bs::plain_tag
                          , bd::scalar_< bd::floating_<A0> >
                          )
  {
    BOOST_FORCEINLINE A0 operator() (const plain_tag &, A0 x) const BOOST_NOEXCEPT
    {
      return musl_(log2)(x); //the "plain" version of the algorithm is never speedier than the "musl" version.
      // the call is here to allow a scalar fallback to simd calls
    }
  };

} } }

#endif


/*  Copyright (C) 2017 NEC Laboratories America, Inc. ("NECLA"). All rights reserved.
 *
 * This source code is licensed under the license found in the LICENSE file in
 * the root directory of this source tree. An additional grant of patent rights
 * can be found in the PATENTS file in the same directory.
 */

#include "optimizelu.hh"
#include "utils.h"
#include "boost/iterator/counting_iterator.hpp"
#include <boost/numeric/conversion/bounds.hpp>  // boost::numeric::bounds<T>
#include <cmath>
#include <iostream>

#include "constants.h"

#define __restricted /* __restricted seems to be an error */

namespace mcsolver_detail{

  using namespace std;
  /** get optimal {l,u} bounds given projection and class order.
   * Computationally expensive, so it should be done sparingly.
   */
  void optimizeLU(VectorXd& l, VectorXd& u,
		  const VectorXd& projection, const SparseMb& y,
		  const vector<int>& class_order, const vector<int>& sorted_class,
		  const VectorXd& wc, const VectorXi& nclasses,
		  const VectorXd& inside_weight, const VectorXd& outside_weight,
		  const boolmatrix& filtered,
		  const double C1, const double C2,
		  const param_struct& params)
  {
#define BOUNDGRAD_THREAD 1 // 1=good

    // -------------- and sanity checks
#if MCTHREADS && defined(_OPENMP)
    if( omp_in_parallel() )
      throw runtime_error(" ERROR: please don't call optimizeLU from and omp parallel section");
#endif
    static bool check_once=false;
    if( !check_once ){
      size_t nErase=0U;
#if MCTHREADS && defined(_OPENMP)
#pragma omp parallel for schedule(static)
#endif
      for(size_t o=0U; o<y.outerSize(); ++o){
	for(SparseMb::InnerIterator it(y,o); it; ++it) {
	  if( it.value()==false ){
	    ++nErase;
	  }
	}
      }
      if( nErase )
	throw runtime_error(" ERROR: SparseMb 'y' should have only 'true' entries");
      if( ! y.isCompressed() )
	throw runtime_error(" ERROR: expect SparseMb 'y' to be compressed sparse");
      check_once=true;
    }
    // ------------------------------------------------------
    size_t const n = projection.size();
    size_t const noClasses = y.cols();
    bool const none_filtered = filtered.count()==0;
    VectorXd allproj(2*n);
    allproj << (projection.array() - 1), (projection.array() + 1);
    std::vector<size_t> indices(allproj.size());
    sort_index(allproj, indices);

    // yes, co[sc[c]] == c
    // initialize the gradients for u and l
    VectorXd gradu(noClasses); // by ranked classes, to minimize cache misses/false sharing
    VectorXd gradl(noClasses); // by ranked classes, to minimize cache misses/false sharing
#if MCTHREADS && defined(_OPENMP)
    size_t const ck = std::max(size_t{256U},size_t{noClasses/omp_get_max_threads()});
#pragma omp parallel for schedule(static,ck)
#endif
    for (size_t sc = 0; sc <noClasses; ++sc) {
      int const c = sorted_class[sc];
      double const classweight = wc.coeff(c);
      if (classweight <= 0.0) { // no examples of this class
	// classes with no weight (wc[]) get {l,u} bounds set to high/low values.
	u.coeffRef(c) = boost::numeric::bounds<double>::lowest()/10; // the /10 is needed because octave has trouble reading back ascii files that are written with the highest/lowest limits
	l.coeffRef(c) = boost::numeric::bounds<double>::highest()/10; // the /10 is needed because octave has trouble reading back ascii files that are written with the highest/lowest limits;
	gradu.coeffRef(sc) = -1.0;
	gradl.coeffRef(sc) = -1.0;
      } else {
	gradu.coeffRef(sc) = C1*classweight;
	gradl.coeffRef(sc) = C1*classweight;
      }
    }

#if BOUNDGRAD_THREAD && MCTHREADS && defined(_OPENMP)
    int const max_n_chunks = omp_get_max_threads();

    int const min_chunk_size = 10000;     // need a test case to set this value XXX
#endif

#if MCTHREADS
#pragma omp parallel default(none) shared(l, u, allproj, indices, filtered, y, class_order, sorted_class, wc, nclasses, inside_weight, outside_weight, params, gradu, gradl)
#endif
    {
#if MCTHREADS
#pragma omp single nowait
#endif
      { // calculate the optimal value for upper bounds -- iterate from beginning
        std::vector<int> classes;
        classes.reserve(nclasses.maxCoeff());
        for (std::vector<size_t>::const_iterator i = indices.begin(); i != indices.end(); ++i) {
	  bool plus = false;
	  size_t idx = *i;
	  if (idx >= n) { plus = true; idx -= n; }

	  if (plus) { // only the upper bounds of the classes of this example are affected
	    double const class_weight = C1*inside_weight.coeff(idx);
	    for (SparseMb::InnerIterator it(y,idx); it; ++it) {
	      int const cs = it.col();                // raw [unsorted] class
	      int const sc = class_order[cs];         // sorted class number
	      if( gradu.coeff(sc) >= 0 && (gradu.coeffRef(sc) -= class_weight) <  std::numeric_limits<double>::epsilon()*class_weight*10){
		u.coeffRef(cs) = allproj.coeff(*i);
	      }
	    }
	  }else{
	    // only the classes ranked lower than the classes of this example are affected
	    sortedClasses( /*OUT*/ classes, /*IN*/ class_order, y, idx );
	    if (classes.size() == 0) continue;
	    if (classes.back() == 0) continue;

	    double const other_weight = C2*outside_weight.coeff(idx);
	    // how many classes of the curent instance should be ranked higher
	    //  times the weight of each
	    //  if each class has its own weight will need to
	    //  be calculated below (or have it precomputed for each example
	    //  as a corresponding wclasses to nclasses to be wclasses the same as wc
	    //  corresponds to nc
	    double const right_update = other_weight * nclasses.coeff(idx);
#if BOUNDGRAD_THREAD && MCTHREADS && defined(_OPENMP)
	    // make sure there is enough work to do to paralelize this
	    int n_chunks = classes.back()/min_chunk_size + 1;
	    n_chunks = n_chunks < max_n_chunks?n_chunks:max_n_chunks;
	    if( n_chunks > 1 ){
	      int chunk_size = classes.back()/n_chunks;
	      int remaining = classes.back()%n_chunks;
	      for (int chunk=0; chunk < n_chunks; ++chunk)
		{
#pragma omp task default(shared) firstprivate(chunk) shared(gradu, u, idx, i, sorted_class, classes, allproj, filtered, chunk_size, remaining)
		  {
		    int sc_start = chunk*chunk_size + (chunk<remaining?chunk:remaining);
		    int sc_incr = chunk_size + (chunk<remaining);
		    getBoundGrad(gradu, u, idx, *i, sorted_class, sc_start, sc_start + sc_incr, classes, right_update, -other_weight, allproj, none_filtered, filtered);
		  }
#pragma omp taskwait // need to wait for all tasks to finish before moving to the next example
		}
	    }else{
	      getBoundGrad(gradu, u, idx, *i, sorted_class, 0,classes.back(),classes,right_update,-other_weight,allproj,none_filtered,filtered);
	    }
#else // not _OPENMP
	    getBoundGrad(gradu, u, idx, *i, sorted_class, 0,classes.back(),classes,right_update,-other_weight,allproj,none_filtered,filtered);
#endif // _OPENMP
	  }
        }
      }

#if MCTHREADS
#pragma omp single nowait
#endif
      { // calculate the optimal value for lower bounds -- iterate from end
        std::vector<int> classes;
        classes.reserve(nclasses.maxCoeff());
        for (std::vector<size_t>::const_reverse_iterator i = indices.rbegin(); i != indices.rend(); i++) {
	  bool plus = false;
	  size_t idx = *i;
	  if (idx >= n) { plus = true; idx -= n; }

	  if (!plus){ // only the lower bounds of the classes of this example are affected
	    double const class_weight = inside_weight.coeff(idx)*C1;
	    for (SparseMb::InnerIterator it(y,idx); it; ++it) {
	      //assert( it.value() ); //if (it.value())
	      int cs = it.col();
	      int sc = class_order[cs];
	      if (gradl.coeff(sc) >= 0 && (gradl.coeffRef(sc) -= class_weight) < std::numeric_limits<double>::epsilon()*class_weight*10){
		l.coeffRef(cs) = allproj.coeff(*i);
	      }
	    }
	  }else{
	    // only the classes ranked higher than the classes of this example are affected
	    // calling y.coeff is expensive so get the classes in the ranked order here
	    sortedClasses( /*OUT*/ classes, /*IN*/ class_order, y, idx );
	    if(classes.size() == 0U) continue;
	    // if a class has lower rank than the lowest rank class of this example
	    // it's lower bound will not be influenced by this example
	    int n_active = noClasses - classes.front() - 1;
	    if (n_active <= 0)
	      continue;

	    double const other_weight = outside_weight.coeff(idx)*C2;
#if BOUNDGRAD_THREAD && MCTHREADS && defined(_OPENMP)
	    // make sure there is enough work to do to paralelize this
	    int n_chunks = n_active/min_chunk_size + 1;
	    n_chunks = n_chunks < max_n_chunks?n_chunks:max_n_chunks;
	    if( n_chunks > 1 ){
	      int chunk_size = n_active/n_chunks;
	      int remaining = n_active%n_chunks;
	      for (int chunk=0; chunk < n_chunks; ++chunk)
		{
#pragma omp task default(shared) firstprivate(chunk) shared(gradl, l, idx, i, sorted_class, classes, allproj, filtered, chunk_size, remaining)
		  {
		    int sc_start = classes.front() + 1 + chunk*chunk_size + (chunk<remaining?chunk:remaining);
		    int sc_incr = chunk_size + (chunk<remaining);
		    getBoundGrad(gradl, l, idx, *i, sorted_class, sc_start, sc_start + sc_incr, classes, 0.0, other_weight, allproj, none_filtered, filtered);
		  }
#pragma omp taskwait  // need to wait for all tasks to finish before moving to the next example
		}
	    }else{
	      getBoundGrad(gradl, l, idx, *i, sorted_class, classes.front() + 1,noClasses,classes, 0.0, other_weight, allproj, none_filtered, filtered);
	    }
#else // not _OPENMP
	    getBoundGrad(gradl, l, idx, *i, sorted_class, classes.front() + 1,noClasses,classes, 0.0, other_weight, allproj, none_filtered, filtered);
#endif // _OPENMP
	  }
        }
      } // omp single
    } // omp parallel
  }
}


/* =========================================================================
   Copyright (c) 2010-2014, Institute for Microelectronics,
                            Institute for Analysis and Scientific Computing,
                            TU Wien.
   Portions of this software are copyright by UChicago Argonne, LLC.

                            -----------------
                  ViennaCL - The Vienna Computing Library
                            -----------------

   Project Head:    Karl Rupp                   rupp@iue.tuwien.ac.at

   (A list of authors and contributors can be found in the PDF manual)

   License:         MIT (X11), see file LICENSE in the base directory
============================================================================= */

/** \example multithreaded.cpp
*
*   This tutorial shows how to use ViennaCL with multiple threads, one thread per GPU.
*   The use of one thread per context (host, CUDA, OpenCL) is supported.
*   However, using more than one thread per context is not fully covered by the OpenCL standard.
*   It is, however, perfectly fine to use multiple OpenCL contexts simultaneously, each using one thread.
*
*   We start with including the necessary headers:
**/

#ifndef VIENNACL_WITH_OPENCL
  #define VIENNACL_WITH_OPENCL
#endif

// include necessary system headers
#include <iostream>

//include basic scalar and vector types of ViennaCL
#include "viennacl/scalar.hpp"
#include "viennacl/vector.hpp"
#include "viennacl/matrix.hpp"

#include "viennacl/ocl/device.hpp"
#include "viennacl/ocl/platform.hpp"
#include "viennacl/ocl/backend.hpp"

//include the generic inner product functions of ViennaCL
#include "viennacl/linalg/norm_2.hpp"

// Some helper functions for this tutorial:
#include "Random.hpp"

#include <boost/thread.hpp>

/**
*  Each thread runs a separate task, for which we provide the following functor.
*  For each thread, vector operations and a vector norm gets computed.
**/
template<typename NumericT>
class worker
{
public:
  worker(std::size_t tid) : thread_id_(tid) {}

  void operator()()
  {
    std::size_t N = 6;

    viennacl::context ctx(viennacl::ocl::get_context(static_cast<long>(thread_id_)));
    viennacl::vector<NumericT> u = viennacl::scalar_vector<NumericT>(N, NumericT(1) * NumericT(thread_id_ + 1), ctx);
    viennacl::vector<NumericT> v = viennacl::scalar_vector<NumericT>(N, NumericT(2) * NumericT(thread_id_ + 1), ctx);
    viennacl::matrix<NumericT> A = viennacl::linalg::outer_prod(u, v);
    viennacl::vector<NumericT> x(u);

    u += v;
    NumericT result = viennacl::linalg::norm_2(u);

    std::stringstream ss;
    ss << "Result of thread " << thread_id_ << " on device " << viennacl::ocl::get_context(static_cast<long>(thread_id_)).devices()[0].name() << ": " << result << std::endl;
    ss << "  A: " << A << std::endl;
    ss << "  x: " << x << std::endl;
    message_ = ss.str();
  }

  std::string message() const { return message_; }

private:
  std::string message_;
  std::size_t thread_id_;
};

/**
*   In the main routine we create two OpenCL contexts and then use one thread per context to run the operations in the functor defined above.
**/
int main()
{
  // Change this type definition to double if your gpu supports that
  typedef float       ScalarType;

  if (viennacl::ocl::get_platforms().size() == 0)
  {
    std::cerr << "Error: No platform found!" << std::endl;
    return EXIT_FAILURE;
  }

  /**
  *   Part 1: Setup first device for first context, second device for second context:
  **/
  viennacl::ocl::platform pf = viennacl::ocl::get_platforms()[0];
  std::vector<viennacl::ocl::device> const & devices = pf.devices();

  // Set first device to first context:
  viennacl::ocl::setup_context(0, devices[0]);

  // Set second device for second context (use the same device for the second context if only one device available):
  if (devices.size() > 1)
    viennacl::ocl::setup_context(1, devices[1]);
  else
    viennacl::ocl::setup_context(1, devices[0]);

  /**
  *   Part 2: Now let two threads operate on two GPUs in parallel.
  **/

  worker<ScalarType> work_functor0(0);
  worker<ScalarType> work_functor1(1);
  boost::thread worker_thread_0(boost::ref(work_functor0));
  boost::thread worker_thread_1(boost::ref(work_functor1));

  worker_thread_0.join();
  worker_thread_1.join();

  std::cout << work_functor0.message() << std::endl;
  std::cout << work_functor1.message() << std::endl;

  /**
  *  We're done - print a success message.
  **/
  std::cout << "!!!! TUTORIAL COMPLETED SUCCESSFULLY !!!!" << std::endl;

  return EXIT_SUCCESS;
}


/*
  uniform_smooth_noise64.hh

  Copyright (c) 2018 Ryan P. Nicholl, "Exaeta", <exaeta@protonmail.com>

    https://github.com/Exaeta/det_noise64

    uniform_smooth_noise64 is a N-dimensional uniform smooth noise
    function, but I'm going to change the hash function into something
    faster/more well known in a later version (and/or allow templating
    the hash used). The next version or version with templated hash will
    be cross platform deterministic.

    It doesn't rely on any third party libraries except boost::multiprecision,
    which can be substituited for any library that provides a 128-bit
    unsigned integer.

    Any donations would be appreciated. :)

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

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

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

*/

#ifndef RPNX_G_NOISE_HH
#define RPNX_G_NOISE_HH

#include <array>
#include <cinttypes>
#include <random>
#include <boost/multiprecision/cpp_int.hpp>

namespace rpnx
{

  using uint64_t = std::uint64_t;
  using uint128_t = boost::multiprecision::uint128_t;

  /*
    This function is probably not cryptographically secure.
    For insecure functions only.
  */
  template <std::size_t N>
  std::uint64_t det_point_noise64(std::array<std::uint64_t, N> inputs, std::size_t c = 5, std::uint64_t seed = 0)
  {
    std::uint64_t output = seed + 0xFAFAFAFAFAFAFAFAull;
    for (std::size_t k = 0; k < c; k++)
    {
      for (std::size_t i = 0; i < N; i++)
      {
        output = (output >> 17) | (output  << (64 - 17));
        output += inputs[i];
        output = (output >> ((k+inputs[i]) &0x0F)) | (output  << (64 - ((k+inputs[i]) &0x0F)));
        output ^= 7;
        output ^= ((output & (0xbeull << 31)) >> 21);
        output += 1;

      }
    }
    output = (output >> 17) | (output  << (64 - 17));

    return output;
  }

  template <std::size_t N>
  bool det_point_noise1(std::array<std::uint64_t, N> inputs)
  {

    std::uint64_t output = 0xFAFAFAFAFAFAFAFAull;
    for (std::size_t k = 0; k < 4; k++)
    {
      for (std::size_t i = 0; i < N; i++)
      {
        output = (output >> 17) | (output  << (64 - 17));
        output += inputs[i];
        output ^= 7;
        output ^= ((output & (0xbeull << 31)) >> 21);
        output += k;
      }
    }
    output = (output >> 19) | (output  << (64 - 19));

    return 1 & ( (output >> 7) ^ (output >> 3) ^ (output >> 5) ^
    (output >> 2) ^ (output >> 11) ^ (output >> 17) ^ (output >> 13) );
  }

  /** A deterministic pseudo-random N-dimensional smooth noise function,
      which, assuming the values of the input and hash function are
      uniformly distributed, provides uniformly distributed output.

      The algorithm used by this function might be changed in a future
      version of this library.

      @param C C is the template parameter that represents the number of
      bits used to interpolate between coordinates. A higher value of C
      will make the noise smooth over a larger range.

      @param N N is the number of elements in the array. Generally leave
      this to automatic deduction.

      @param inputs A std::array of N 64-bit integers

      @time O(N 2^N), O(1) as compiled

      @space O(2^N),  O(1) as compiled
  */
  template <std::size_t C = 32, std::size_t N = 1>
  std::uint64_t wave_noise64(std::array<std::uint64_t, N> inputs, std::uint64_t seed = 0)
  {
    using u64 = std::uint64_t;
    using u128 = uint128_t;
    using std::uint8_t;

    u64 out=0;

    std::array<uint64_t, (1 << N)> field_corners;
    // 2 ^ N dimensions
    // gathers the values in each corner.

    std::mt19937 rg;

    std::uint8_t r = 0; // used later;

    std::array<std::uint64_t, N> field_inputs = inputs;
    // Each dimension has a set of inputs

    for (std::size_t k = 0; k < field_inputs.size(); k++)
    {
      field_inputs[k] >>= C;

      // only the first N-C bits are significant to the field

    }

    for (std::size_t i = 0; i < field_corners.size(); i++)
    {

      std::array<std::uint64_t, N> field_inputs2 = field_inputs;

      for (std::size_t k = 0; k < N; k++)
      {
        if (i & (1 << k)) field_inputs2[k]++;
        // We must adjust the field inputs depending on which "corners"
        // we are in.
      }

      field_corners[i] = det_point_noise64(field_inputs2, 8, seed);
      // calculate the corner values

      r += field_corners[i] & 0xFF;
      // replace lost randomness with this value;

    }

   // std::cout << "intial corner[0]=" << field_corners[0] << std::endl;

    std::array<std::uint64_t, N> sigvals;
    std::array<bool, N> bvals;
    for (std::size_t i = 0; i < N; i++)
    {
      sigvals[i] = inputs[i] & ((std::uint64_t(1) << C) - 1);
      bvals[i] = 1 & inputs[i] >> C;
    }
    // We need to know the dimensional significance of dimension in
    // order to interpolate the values correctly.

    // The following code interpolates the N-dimensional structure
    // in N log N time by collapsing each dimension one at a time
    // until the structure is 0-dimensional and has only 1 value

    std::size_t q = N;
    // q here is to avoid taking logs later

    out = 0;

    for (size_t z = 0; z < N; z++)
    {
      u64 a = det_point_noise64(std::array<u64, 1>{field_inputs[z]}, 5, z+seed);
      u64 b = det_point_noise64(std::array<u64, 1>{field_inputs[z]+1}, 5, z+seed);

      u64 v1 = sigvals[z];
      u64 v2 = (u64(1) << C) - v1;

      //std::swap(a, b);
      if (bvals[z])
      {
        std::swap(a, b);
        std::swap(v1, v2);
      }

      u64 m1 = static_cast<u64>( ((u128(b) * v1) + (u128(a) * v2)) >> C );

      out += 0;
      out += m1;

    }

     if (out & (std::uint64_t(1) << 63))
    {
      out = ((~out ^ 1) << 1) | 1;
    }
    else out = out << 1;

    /* The transformation above is supposed to redistribute the "out"
    value such that values near 0 are "similar" so there aren't abrupt
    transitions due to modular arithmetic wrapping from 1 to 0.
    */

    out ^= 1 & (r ^ (r >> 3) ^ (r >> 5));
    /*
      An unfortunate result of the above transformation is that although
      it's uniform over the 64-bit space, the last bit only changes when
      the sign flips and thus effectively only 63 bits are adequately
      random. To fix that some "random" bits are used.
    */

    return out;

  }

    template <typename R>
  void wrap_correction(R & r, uint64_t & val)
  {
    if (val & (std::uint64_t(1) << 63))
    {
      val = ((~val ^ 1) << 1) | 1;
    }
    else val = val << 1;

    val ^= 1 & r();
  }

  template <std::size_t C = 32, std::size_t D = 2, std::size_t N = 1>
  std::uint64_t crystal_noise64(std::array<std::uint64_t, N> inputs)
  {
    using u64 = std::uint64_t;

    static_assert(64 - C - D >= 0 && 64 - C - D <= 63, "Invalid parameters");

    u64 out = 0;
    std::array<u64, N+1> a;

    for (int i = 0; i < N; i++)
    {
      a[i] = inputs[i];
    }
    a[N] = 0;

    std::array<u64, N> field_input = inputs;
    for (int i = 0; i < N; i++)
    {
      field_input[i] = inputs[i];
    }

    if (true) for (u64 i = 0; i < N; i++)
    {
      std::array<u64, N> b = inputs;

      u64 k = det_point_noise64(std::array<u64, N>{i}, 5, i);
      for (size_t i2 = 0; i2 < N; i2++)
      {
        b[i2] += k;
      }

      field_input[i] += (wave_noise64<C>(b)) >> (64 - C - D) ;

    }

    out = wave_noise64<C>(field_input);

    return out;
  }

  template <std::size_t C = 32, std::size_t D = 2, std::size_t N = 1>
  std::uint64_t bicrystal_noise64(std::array<std::uint64_t, N> inputs)
  {
    using u64 = std::uint64_t;

    static_assert(64 - C - D >= 0 && 64 - C - D <= 63, "Invalid parameters");

    std::array<u64, N+1> a;
    for (int i = 0; i < N; i++)
    {
      a[i] = inputs[i];
    }
    a[N] = 0;

    std::array<u64, N> field_input = inputs;
    for (int i = 0; i < N; i++)
    {
      field_input[i] = inputs[i];
    }

    if (true) for (u64 i = 0; i < N; i++)
    {
      std::array<u64, N> b = inputs;

      u64 k = det_point_noise64(std::array<u64, N>{i});
      for (size_t i2 = 0; i2 < N; i2++)
      {
        b[i2] += k;
      }

      field_input[i] += (crystal_noise64<C>(b)) >> (64 - C - D) ;

    }

    return wave_noise64<C>(field_input);
  }

  template <std::size_t C = 12, size_t N = 1>
  uint64_t weave_noise2d_64(std::array<uint64_t, N> inputs, bool correct = true, size_t d = 1)
  {
    std::mt19937_64 r{0};
    std::uint64_t output = 0;

    const constexpr std::size_t K = 3;
    const constexpr std::size_t k = (1 << K);

    std::array<uint64_t, N + 2> k_inputs;

    for (size_t i = 0; i < N; i++)
    {
      k_inputs[i] = inputs[i];
    }

    k_inputs[N] = r();
     k_inputs[N+1] = r();

    std::uint64_t q = (1 << (C));

    for (std::size_t i = 0; i < k; i++) for (std::size_t j = 0; j < k; j++)
    {

      std::array<std::uint64_t, N+2> y_inputs = k_inputs;
      y_inputs[N] = r();
      y_inputs[N+1] = r();
      y_inputs[0] += q*i;
      y_inputs[1] += q*j;

      uint64_t km = crystal_noise64<C+K, 1>(y_inputs);
      km = km/d;
      output += km;
    }

    if (correct) wrap_correction(r, output);

    return output;

  }

}

#endif


/*
 Copyright (C) 2020 Quaternion Risk Management Ltd
 All rights reserved.

 This file is part of ORE, a free-software/open-source library
 for transparent pricing and risk analysis - http://opensourcerisk.org

 ORE is free software: you can redistribute it and/or modify it
 under the terms of the Modified BSD License.  You should have received a
 copy of the license along with this program.
 The license is also available online at <http://opensourcerisk.org>

 This program is distributed on the basis that it will form a useful
 contribution to risk analytics and model standardisation, but WITHOUT
 ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 FITNESS FOR A PARTICULAR PURPOSE. See the license for more details.
*/

/*! \file qle/termstructures/blackvolsurfacewithatm.hpp
    \brief Wrapper class for a BlackVolTermStructure that easily exposes ATM vols.
    \ingroup termstructures
*/

#ifndef quantext_blackvolsurfacewithatm_hpp
#define quantext_blackvolsurfacewithatm_hpp

#include <boost/shared_ptr.hpp>
#include <ql/termstructures/volatility/equityfx/blackvoltermstructure.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <qle/indexes/equityindex.hpp>

namespace QuantExt {
using namespace QuantLib;

//! Wrapper class for a BlackVolTermStructure that allows us to proxy one equity vol surface off another.
/*! This class implements BlackVolatilityTermStructure and takes a surface (well, any BlackVolTermStructure) as an
    input. It also takes Handles to two EquityIndices (index and proxyIndex), where index is the 'EquityIndex' of the
   underlying for the surface being constructed and proxyIndex is the 'EquityIndex' for the surface being proxied off.

    The vol returned from the new surface is proxied from the base, adjusting by the forward prices to match ATM:

    \f{eqnarray}{
    \sigma_2(K,T) = \sigma_1(\frac{K}{F_2}*F_1,T)
    \f}

    Where \n\n
    \f$ \sigma_1 = \text{Volatility of underlying being proxied against}\f$ \n
    \f$ \sigma_2 = \text{Volatility of underlying being proxied}\f$ \n
    \f$ F_1 = \text{Forward at time T of the underlying being proxied against}\f$ \n
    \f$ F_2 = \text{Forward at time T of the underlying being proxied}\f$ \n
    \f$ T = \text{Time}\f$
    \n

    Note: This surface only proxies equity volatilities, this is because we are forced to look up the equity fixings
    using time instead of date and use the forecastFixing method in an EquityIndex. A more general class could be
   developed if need, using Index instead of EquityIndex, if the time lookup could be overcome.

    */
//!\ingroup termstructures

class EquityBlackVolatilitySurfaceProxy : public BlackVolatilityTermStructure {
public:
    //! Constructor. This is a floating term structure (settlement days is zero)
    EquityBlackVolatilitySurfaceProxy(const boost::shared_ptr<BlackVolTermStructure>& proxySurface,
                                      const boost::shared_ptr<EquityIndex>& index,
                                      const boost::shared_ptr<EquityIndex>& proxyIndex);

    //! \name TermStructure interface
    //@{
    DayCounter dayCounter() const { return proxySurface_->dayCounter(); }
    Date maxDate() const { return proxySurface_->maxDate(); }
    Time maxTime() const { return proxySurface_->maxTime(); }
    const Date& referenceDate() const { return proxySurface_->referenceDate(); }
    Calendar calendar() const { return proxySurface_->calendar(); }
    Natural settlementDays() const { return proxySurface_->settlementDays(); }
    //@}

    //! \name VolatilityTermStructure interface
    //@{
    Rate minStrike() const;
    Rate maxStrike() const;
    //@}

    //! \name Inspectors
    //@{
    boost::shared_ptr<BlackVolTermStructure> proxySurface() const { return proxySurface_; }
    boost::shared_ptr<EquityIndex> index() const { return index_; }
    boost::shared_ptr<EquityIndex> proxyIndex() const { return proxyIndex_; }
    //@}

protected:
    // Here we adjust the returned vol.
    Volatility blackVolImpl(Time t, Real strike) const;

private:
    boost::shared_ptr<BlackVolTermStructure> proxySurface_;
    boost::shared_ptr<EquityIndex> index_, proxyIndex_;
};

} // namespace QuantExt

#endif


//=======================================================================
// Copyright 1997, 1998, 1999, 2000 University of Notre Dame.
// Copyright 2004, 2005 Trustees of Indiana University
// Authors: Andrew Lumsdaine, Lie-Quan Lee, Jeremy G. Siek,
//          Doug Gregor, D. Kevin McGrath
//
// Distributed under the Boost Software License, Version 1.0. (See
// accompanying file LICENSE_1_0.txt or copy at
// http://www.boost.org/LICENSE_1_0.txt)
//=======================================================================
#ifndef BOOST_GRAPH_CUTHILL_MCKEE_HPP
#define BOOST_GRAPH_CUTHILL_MCKEE_HPP

#include <boost/config.hpp>
#include <boost/graph/detail/sparse_ordering.hpp>
#include <boost/graph/graph_utility.hpp>
#include <algorithm>

/*
  (Reverse) Cuthill-McKee Algorithm for matrix reordering
*/

namespace boost {

  namespace detail {

    template < typename OutputIterator, typename Buffer, typename DegreeMap >
    class bfs_rcm_visitor:public default_bfs_visitor
    {
    public:
      bfs_rcm_visitor(OutputIterator *iter, Buffer *b, DegreeMap deg):
        permutation(iter), Qptr(b), degree(deg) { }
      template <class Vertex, class Graph>
      void examine_vertex(Vertex u, Graph&) {
        *(*permutation)++ = u;
        index_begin = (int)Qptr->size();
      }
      template <class Vertex, class Graph>
      void finish_vertex(Vertex, Graph&) {
        using std::sort;

        typedef typename property_traits<DegreeMap>::value_type ds_type;

        typedef indirect_cmp<DegreeMap, std::less<ds_type> > Compare;
        Compare comp(degree);

        sort(Qptr->begin()+index_begin, Qptr->end(), comp);
      }
    protected:
      OutputIterator *permutation;
      int index_begin;
      Buffer *Qptr;
      DegreeMap degree;
    };

  } // namespace detail

  // Reverse Cuthill-McKee algorithm with a given starting Vertex.
  //
  // If user provides a reverse iterator, this will be a reverse-cuthill-mckee
  // algorithm, otherwise it will be a standard CM algorithm

  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap>
  OutputIterator
  cuthill_mckee_ordering(const Graph& g,
                         std::deque< typename
                         graph_traits<Graph>::vertex_descriptor > vertex_queue,
                         OutputIterator permutation,
                         ColorMap color, DegreeMap degree)
  {

    //create queue, visitor...don't forget namespaces!
#ifdef __GNUC__
    typedef typename property_traits<DegreeMap>::value_type ds_type __attribute__((unused));
#else
    typedef typename property_traits<DegreeMap>::value_type ds_type;
#endif
    typedef typename graph_traits<Graph>::vertex_descriptor Vertex;
    typedef typename boost::sparse::sparse_ordering_queue<Vertex> queue;
    typedef typename detail::bfs_rcm_visitor<OutputIterator, queue, DegreeMap> Visitor;
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    queue Q;

    //create a bfs_rcm_visitor as defined above
    Visitor     vis(&permutation, &Q, degree);

    typename graph_traits<Graph>::vertex_iterator ui, ui_end;

    // Copy degree to pseudo_degree
    // initialize the color map
    for (boost::tie(ui, ui_end) = vertices(g); ui != ui_end; ++ui){
      put(color, *ui, Color::white());
    }

    while( !vertex_queue.empty() ) {
      Vertex s = vertex_queue.front();
      vertex_queue.pop_front();

      //call BFS with visitor
      breadth_first_visit(g, s, Q, vis, color);
    }
    return permutation;
  }

  // This is the case where only a single starting vertex is supplied.
  template <class Graph, class OutputIterator,
            class ColorMap, class DegreeMap>
  OutputIterator
  cuthill_mckee_ordering(const Graph& g,
                         typename graph_traits<Graph>::vertex_descriptor s,
                         OutputIterator permutation,
                         ColorMap color, DegreeMap degree)
  {

    std::deque< typename graph_traits<Graph>::vertex_descriptor > vertex_queue;
    vertex_queue.push_front( s );

    return cuthill_mckee_ordering(g, vertex_queue, permutation, color, degree);

  }

  // This is the version of CM which selects its own starting vertex
  template < class Graph, class OutputIterator,
             class ColorMap, class DegreeMap>
  OutputIterator
  cuthill_mckee_ordering(const Graph& G, OutputIterator permutation,
                         ColorMap color, DegreeMap degree)
  {
    if (boost::graph::has_no_vertices(G))
      return permutation;

    typedef typename boost::graph_traits<Graph>::vertex_descriptor Vertex;
#ifdef __GNUC__
    typedef typename boost::graph_traits<Graph>::vertex_iterator   VerIter __attribute__((unused));
#else
    typedef typename boost::graph_traits<Graph>::vertex_iterator   VerIter;
#endif
    typedef typename property_traits<ColorMap>::value_type ColorValue;
    typedef color_traits<ColorValue> Color;

    std::deque<Vertex>      vertex_queue;

    // Mark everything white
    BGL_FORALL_VERTICES_T(v, G, Graph) put(color, v, Color::white());

    // Find one vertex from each connected component
    BGL_FORALL_VERTICES_T(v, G, Graph) {
      if (get(color, v) == Color::white()) {
        depth_first_visit(G, v, dfs_visitor<>(), color);
        vertex_queue.push_back(v);
      }
    }

    // Find starting nodes for all vertices
    // TBD: How to do this with a directed graph?
    for (typename std::deque<Vertex>::iterator i = vertex_queue.begin();
         i != vertex_queue.end(); ++i)
      *i = find_starting_node(G, *i, color, degree);

    return cuthill_mckee_ordering(G, vertex_queue, permutation,
                                  color, degree);
  }

  template<typename Graph, typename OutputIterator, typename VertexIndexMap>
  OutputIterator
  cuthill_mckee_ordering(const Graph& G, OutputIterator permutation,
                         VertexIndexMap index_map)
  {
    if (boost::graph::has_no_vertices(G))
      return permutation;

#ifdef __GNUC__
    typedef out_degree_property_map<Graph> DegreeMap __attribute__((unused));
#else
    typedef out_degree_property_map<Graph> DegreeMap;
#endif
    std::vector<default_color_type> colors(num_vertices(G));
    return cuthill_mckee_ordering(G, permutation,
                                  make_iterator_property_map(&colors[0],
                                                             index_map,
                                                             colors[0]),
                                  make_out_degree_map(G));
  }

  template<typename Graph, typename OutputIterator>
  inline OutputIterator
  cuthill_mckee_ordering(const Graph& G, OutputIterator permutation)
  { return cuthill_mckee_ordering(G, permutation, get(vertex_index, G)); }
} // namespace boost

#endif // BOOST_GRAPH_CUTHILL_MCKEE_HPP


/*
//@HEADER
// ************************************************************************
//
// qr_tpetra_mv_householder_using_eigen_impl.hpp
//                     		  Pressio
//                             Copyright 2019
//    National Technology & Engineering Solutions of Sandia, LLC (NTESS)
//
// Under the terms of Contract DE-NA0003525 with NTESS, the
// U.S. Government retains certain rights in this software.
//
// Pressio is licensed under BSD-3-Clause terms of use:
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions
// are met:
//
// 1. Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
//
// 2. Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. Neither the name of the copyright holder nor the names of its
// contributors may be used to endorse or promote products derived
// from this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
// FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
// COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
// INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
// SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
// HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
// STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING
// IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Questions? Contact Francesco Rizzi (fnrizzi@sandia.gov)
//
// ************************************************************************
//@HEADER
*/

#ifndef QR_IMPL_TPETRA_QR_TPETRA_MV_HOUSEHOLDER_USING_EIGEN_IMPL_HPP_
#define QR_IMPL_TPETRA_QR_TPETRA_MV_HOUSEHOLDER_USING_EIGEN_IMPL_HPP_

#include <Eigen/OrderingMethods>
#include <Eigen/SparseQR>
#include <Tpetra_Map.hpp>
#include <Tpetra_Vector.hpp>

namespace pressio{ namespace qr{ namespace impl{

template<typename MatrixType, typename R_t>
class TpetraMVHouseholderUsingEigen
{

public:
  using sc_t = typename ::pressio::Traits<MatrixType>::scalar_type;
  using lo_t         = typename ::pressio::Traits<MatrixType>::local_ordinal_type;
  using go_t         = typename ::pressio::Traits<MatrixType>::global_ordinal_type;
  using node_t       = typename ::pressio::Traits<MatrixType>::node_type;
  using Q_type       = Tpetra::MultiVector<sc_t, lo_t, go_t, node_t>;

  using eig_dyn_mat	= Eigen::Matrix<sc_t, -1, -1>;
  using help_impl_t	= QRHouseholderDenseEigenMatrix<eig_dyn_mat, R_t>;
  help_impl_t myImpl_	= {};

public:
  TpetraMVHouseholderUsingEigen() = default;
  ~TpetraMVHouseholderUsingEigen() = default;

  template < typename vector_in_t, typename vector_out_t>
  void applyQTranspose(const vector_in_t & vecIn, vector_out_t & vecOut) const
  {
    constexpr auto beta  = ::pressio::utils::Constants<sc_t>::zero();
    constexpr auto alpha = ::pressio::utils::Constants<sc_t>::one();
    ::pressio::ops::product(::pressio::transpose(), alpha, *this->Qmat_, vecIn, beta, vecOut);
  }

  template <typename VectorType>
  void doLinSolve(const VectorType & rhs, VectorType & y)const{
    myImpl_.template doLinSolve<VectorType>(rhs, y);
  }

  const Q_type & QFactor() const {
    return *this->Qmat_;
  }

  void computeThinOutOfPlace(const MatrixType & Ain)
  {
    auto & A = const_cast<MatrixType &>(Ain);

    auto rows = ::pressio::ops::extent(A,0);
    auto cols = ::pressio::ops::extent(A,1);
    auto ArowMap = A.getMap();
    Teuchos::RCP<const Teuchos::Comm<int> > comm =
      Teuchos::rcp (new Teuchos::MpiComm<int> (MPI_COMM_SELF));

    // convert it to replicated eptra matrix
    using local_map_t = Tpetra::Map<lo_t, go_t, node_t>;
    using rcp_local_map_t = Teuchos::RCP<const local_map_t>;
    rcp_local_map_t rcp_local_map = Teuchos::rcp( new local_map_t(rows, 0, comm) );

    using import_t = Tpetra::Import<lo_t, go_t, node_t>;
    import_t importer(ArowMap, rcp_local_map);
    MatrixType A2(rcp_local_map, cols);
    A2.doImport(A, importer, Tpetra::INSERT);

    // store it into an Eigen matrix
    Eigen::Matrix<sc_t, -1, -1> eA2W(rows,cols);
    for (int j=0;j<cols;j++)
    {
      auto colData = A2.getData(j);
      for (int i=0;i<rows;i++){
    	 eA2W(i,j) = colData[i];
      }
    }

    myImpl_.computeThinOutOfPlace(eA2W);

    // store Q into replicated Tpetra::Multivector
    const auto & Q2 = myImpl_.QFactor();
    Q_type locQ( rcp_local_map, Q2.cols() );
    // auto trilD = locQ.data();
    locQ.template sync<Kokkos::HostSpace>();

    auto v2d = locQ.template getLocalView<Kokkos::HostSpace>();
    auto c0 = Kokkos::subview(v2d, Kokkos::ALL(), 0);
    // //we are going to change the host view
    locQ.template modify<Kokkos::HostSpace>();
    for (int i=0;i<Q2.rows();i++)
      for (int j=0;j<Q2.cols();j++)
    	v2d(i,j) = Q2(i,j);

    // import from local to distributed
    Qmat_ = std::make_shared<Q_type>(ArowMap, Q2.cols());
    import_t importer2(rcp_local_map, ArowMap);
    Qmat_->doImport(locQ, importer2, Tpetra::INSERT);
  }

private:
  // todo: these must be moved somewhere else
  mutable std::shared_ptr<Q_type> Qmat_	= nullptr;
  mutable std::shared_ptr<R_t> Rmat_	= nullptr;

};//end class

}}} // end namespace pressio::qr::impl
#endif  // QR_IMPL_TPETRA_QR_TPETRA_MV_HOUSEHOLDER_USING_EIGEN_IMPL_HPP_


//============================================================================
// Name        : IntervalAnalyzer.cpp
// Author      : Tomasz Hoffmann
// Version     :
// Copyright   : TH
// Description : Analyzer for interval results
//============================================================================

#include <iostream>
#include <iomanip>
#include <fstream>
#include <time.h>
#include <string>
#include <vector>
#include <limits.h>
#include <boost/lexical_cast.hpp>
#include <boost/filesystem.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/exception/diagnostic_information.hpp>

using namespace std;

namespace alg = boost::algorithm;
namespace fs = boost::filesystem;

int main(int ac, char *av[])
{
	long double eps = std::numeric_limits<long double>::epsilon();
	try
	{

		if (ac != 7)
		{
			cout << "Interval Analyzer" << endl;
			cout << "Usage: \n" << fs::basename(av[0]) << " <l_filename>"
					<< " <r_filename>" << " <e_filename>" << " <f_filename>"
					<< " <oe_filename" << " <of_filename>" << endl << endl;
			cout << "where: \n" << "l_ - left ends of interval solution \n"
					<< "r_ - right ends of interval solution \n"
					<< "e_ - exact solution (if known) \n"
					<< "f_ - fp arithmetic solutions \n"
					<< "oe_ - exact distribution output file name \n"
					<< "of_ - fp distribution output file name \n" << endl;
			return 0;
		}
		ifstream l_file(av[1]);
		ifstream r_file(av[2]);
		ifstream e_file(av[3]);
		ifstream f_file(av[4]);
		fstream oe_file(av[5], fstream::out);
		fstream of_file(av[6], fstream::out);

		//string fname = av[4];
		//		o_file.open(fname.c_str(), fstream::out);

		int dprec = std::numeric_limits<long double>::digits10;

		bool allopen = true;
		allopen &= l_file.is_open();
		allopen &= r_file.is_open();
		allopen &= e_file.is_open();
		allopen &= f_file.is_open();

		if (!allopen)
		{
			cout << "One or more files cannot be open! \n";
			return 0;
		}
		else
		{
			string line;
			vector<string> v;
			vector<long double> lv;
			vector<long double> rv;
			vector<long double> ev;
			vector<long double> fv;

			while ((!l_file.eof()) && (!r_file.eof()) && (!e_file.eof())
					&& (!f_file.eof()))
			{
				getline(l_file, line);
				alg::split(v, line, alg::is_any_of(";"));
				vector<string>::iterator it;
				if (v.size() < 2)
					break;
//				cout << "Start left..." << endl;
				for (it = v.begin(); it != v.end(); ++it)
				{
					lv.push_back(boost::lexical_cast<long double>(*it));
				}
//				cout << "Read left done." << endl;

				getline(r_file, line);
//				cout << "Start right..." << endl;
				alg::split(v, line, alg::is_any_of(";"));
				if (v.size() < 2)
					break;
				for (it = v.begin(); it != v.end(); ++it)
				{
					rv.push_back(boost::lexical_cast<long double>(*it));
				}
//				cout << "Read right done." << endl;

				getline(e_file, line);
//				cout << "Start exact..." << endl;
				if (v.size() < 2)
					break;
				alg::split(v, line, alg::is_any_of(";"));
				for (it = v.begin(); it != v.end(); ++it)
				{
//					cout << (*it);
					ev.push_back(boost::lexical_cast<long double>(*it));
				}
//				cout << "Read exact done." << endl;

				getline(f_file, line);
				alg::split(v, line, alg::is_any_of(";"));
				for (it = v.begin(); it != v.end(); ++it)
				{
					fv.push_back(boost::lexical_cast<long double>(*it));
				}
//				cout << "Read floating-point done." << endl;

				if ((lv.size() == rv.size()) && (rv.size() == ev.size())
						&& (ev.size() == fv.size()))
				{
					long double l, r, e, f, w1, w, m;

					vector<long double> res;
					//caluclate exact solutions distribution
					for (size_t i = 0; i < lv.size(); ++i)
					{
						l = lv.at(i);
						r = rv.at(i);
						e = ev.at(i);
						w = r - l;
						m = (r + l) / 2;
						if ((w < eps) || (e == r) || (e == l))
						{
							res.push_back(0.5);
							continue;
						}
						w1 = abs(e - m);
						if ((w1 / w) > 0.5)
						{
							cout << " \n Wrong element: "
									<< std::setprecision(dprec) << e;
							cout << std::setprecision(dprec) << "\n [ " << l
									<< " ; " << r << " ] \n";
							cout << std::setprecision(dprec) << "m = " << m
									<< " \n";
						}
						res.push_back(w1 / w);
					}

					stringstream ss;
					vector<long double>::iterator it;
					for (it = res.begin(); it != res.end(); ++it)
					{
						ss << std::setprecision(dprec) << (*it) << ";";
					}
					ss << endl;
					oe_file << ss.str();

					//caluclate floating-pint solutions distribution
					res.clear();
					for (size_t i = 0; i < lv.size(); ++i)
					{
						l = lv.at(i);
						r = rv.at(i);
						f = fv.at(i);
						w = r - l;
						m = (r + l) / 2;
						if ((w < eps) || (f == r) || (f == l))
						{
							res.push_back(0.5);
							continue;
						}
						w1 = abs(f - m);
						if ((w1 / w) > 0.5)
						{
							cout << " \n Wrong element: "
									<< std::setprecision(dprec) << e;
							cout << std::setprecision(dprec) << "\n [ " << l
									<< " ; " << r << " ] \n";
							cout << std::setprecision(dprec) << "m = " << m
									<< " \n";
						}
						res.push_back(w1 / w);
					}

					ss.str(string());
					for (it = res.begin(); it != res.end(); ++it)
					{
						ss << std::setprecision(dprec) << (*it) << ";";
					}
					ss << endl;
					of_file << ss.str();

					//clear input vectors for current row
					lv.clear();
					rv.clear();
					ev.clear();
					fv.clear();
				}
			}

		}
		l_file.close();
		r_file.close();
		e_file.close();
		oe_file.close();
	} catch (boost::exception const& ex)
	{
		cout
				<< "Execution problem "
						+ boost::current_exception_diagnostic_information();
	}
	return 0;
}


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

/*
 Copyright (C) 2013 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 gaussian1dmodel.hpp
    \brief basic interface for one factor interest rate models
*/

// uncomment to enable NTL support (see below for more details and references)
// #define GAUSS1D_ENABLE_NTL

#ifndef quantlib_gaussian1dmodel_hpp
#define quantlib_gaussian1dmodel_hpp

#include <ql/models/model.hpp>
#include <ql/models/parameter.hpp>
#include <ql/math/interpolation.hpp>
#include <ql/math/interpolations/cubicinterpolation.hpp>
#include <ql/indexes/iborindex.hpp>
#include <ql/indexes/swapindex.hpp>
#include <ql/instruments/vanillaswap.hpp>
#include <ql/time/date.hpp>
#include <ql/time/period.hpp>
#include <ql/termstructures/yieldtermstructure.hpp>
#include <ql/stochasticprocess.hpp>
#include <ql/utilities/null.hpp>
#include <ql/patterns/lazyobject.hpp>

#ifdef GAUSS1D_ENABLE_NTL
    #include <boost/math/bindings/rr.hpp>
#endif

#include <boost/math/special_functions/erf.hpp>

namespace QuantLib {

    /*! One factor interest rate model interface class
        The only methods that must be implemented by subclasses
        are the numeraire and zerobond methods for an input array
        of state variable values. The variable $y$ is understood
        to be the standardized (zero mean, unit variance) version
        of the model's original state variable $x$.

        NTL support may be enabled by defining GAUSS1D_ENABLE_NTL in this
        file. For details on NTL see
                 http://www.shoup.net/ntl/

        \warning the variance of the state process conditional on
        $x(t)=x$ must be independent of the value of $x$

    */

    class Gaussian1dModel : public TermStructureConsistentModel,
                            public LazyObject {

      public:

        const boost::shared_ptr<StochasticProcess1D> stateProcess() const;

        const Real numeraire(const Time t, const Real y = 0.0,
                             const Handle<YieldTermStructure> &yts =
                                 Handle<YieldTermStructure>()) const;

        const Real zerobond(const Time T, const Time t = 0.0,
                            const Real y = 0.0,
                            const Handle<YieldTermStructure> &yts =
                                Handle<YieldTermStructure>()) const;

        const Real numeraire(const Date &referenceDate, const Real y = 0.0,
                             const Handle<YieldTermStructure> &yts =
                                 Handle<YieldTermStructure>()) const;

        const Real zerobond(const Date &maturity,
                            const Date &referenceDate = Null<Date>(),
                            const Real y = 0.0,
                            const Handle<YieldTermStructure> &yts =
                                Handle<YieldTermStructure>()) const;

        const Real zerobondOption(
            const Option::Type &type, const Date &expiry, const Date &valueDate,
            const Date &maturity, const Rate strike,
            const Date &referenceDate = Null<Date>(), const Real y = 0.0,
            const Handle<YieldTermStructure> &yts =
                Handle<YieldTermStructure>(),
            const Real yStdDevs = 7.0, const Size yGridPoints = 64,
            const bool extrapolatePayoff = true,
            const bool flatPayoffExtrapolation = false) const;

        const Real forwardRate(const Date &fixing,
                               const Date &referenceDate = Null<Date>(),
                               const Real y = 0.0,
                               boost::shared_ptr<IborIndex> iborIdx =
                                   boost::shared_ptr<IborIndex>()) const;

        const Real swapRate(const Date &fixing, const Period &tenor,
                            const Date &referenceDate = Null<Date>(),
                            const Real y = 0.0,
                            boost::shared_ptr<SwapIndex> swapIdx =
                                boost::shared_ptr<SwapIndex>()) const;

        const Real swapAnnuity(const Date &fixing, const Period &tenor,
                               const Date &referenceDate = Null<Date>(),
                               const Real y = 0.0,
                               boost::shared_ptr<SwapIndex> swapIdx =
                                   boost::shared_ptr<SwapIndex>()) const;

        /*! Computes the integral
        \f[ {2\pi}^{-0.5} \int_{a}^{b} p(x) \exp{-0.5*x*x} \mathrm{d}x \f]
        with
        \f[ p(x) = ax^4+bx^3+cx^2+dx+e \f].
        */
        const static Real gaussianPolynomialIntegral(const Real a, const Real b,
                                                     const Real c, const Real d,
                                                     const Real e,
                                                     const Real x0,
                                                     const Real x1);

        /*! Computes the integral
        \f[ {2\pi}^{-0.5} \int_{a}^{b} p(x) \exp{-0.5*x*x} \mathrm{d}x \f]
        with
        \f[ p(x) = a(x-h)^4+b(x-h)^3+c(x-h)^2+d(x-h)+e \f].
        */
        const static Real gaussianShiftedPolynomialIntegral(
            const Real a, const Real b, const Real c, const Real d,
            const Real e, const Real h, const Real x0, const Real x1);

        /*! Generates a grid of values for the standardized state variable $y$
           at time $T$
            conditional on $y(t)=y$, covering yStdDevs standard deviations
           consisting of
            2*gridPoints+1 points */

        const Disposable<Array> yGrid(const Real yStdDevs, const int gridPoints,
                                      const Real T = 1.0, const Real t = 0,
                                      const Real y = 0) const;

      protected:

        // we let derived classes register with the termstructure
        Gaussian1dModel(const Handle<YieldTermStructure> &yieldTermStructure)
            : TermStructureConsistentModel(yieldTermStructure) {}

        virtual ~Gaussian1dModel() {}

        virtual const Real
        numeraireImpl(const Time t, const Real y,
                      const Handle<YieldTermStructure> &yts) const = 0;

        virtual const Real
        zerobondImpl(const Time T, const Time t, const Real y,
                     const Handle<YieldTermStructure> &yts) const = 0;

        void performCalculations() const {}

        void generateArguments() {
            calculate();
            notifyObservers();
        }

        boost::shared_ptr<StochasticProcess1D> stateProcess_;
    };

    inline const boost::shared_ptr<StochasticProcess1D>
    Gaussian1dModel::stateProcess() const {

        QL_REQUIRE(stateProcess_ != NULL, "state process not set");
        return stateProcess_;

    }

    inline const Real
    Gaussian1dModel::numeraire(const Time t, const Real y,
                               const Handle<YieldTermStructure> &yts) const {

        return numeraireImpl(t, y, yts);
    }

    inline const Real
    Gaussian1dModel::zerobond(const Time T, const Time t, const Real y,
                              const Handle<YieldTermStructure> &yts) const {

        return zerobondImpl(T, t, y, yts);
    }

    inline const Real
    Gaussian1dModel::numeraire(const Date &referenceDate, const Real y,
                               const Handle<YieldTermStructure> &yts) const {

        return numeraire(termStructure()->timeFromReference(referenceDate), y,
                         yts);
    }

    inline const Real
    Gaussian1dModel::zerobond(const Date &maturity, const Date &referenceDate,
                              const Real y,
                              const Handle<YieldTermStructure> &yts) const {

        return zerobond(termStructure()->timeFromReference(maturity),
                        referenceDate != Null<Date>()
                            ? termStructure()->timeFromReference(referenceDate)
                            : 0.0,
                        y, yts);
    }

}

#endif


//==============================================================================
//         Copyright 2003 - 2011 LASMEA UMR 6602 CNRS/Univ. Clermont II
//         Copyright 2009 - 2011 LRI    UMR 8623 CNRS/Univ Paris Sud XI
//
//          Distributed under the Boost Software License, Version 1.0.
//                 See accompanying file LICENSE.txt or copy at
//                     http://www.boost.org/LICENSE_1_0.txt
//==============================================================================
#ifndef BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ADDS_HPP_INCLUDED
#define BOOST_SIMD_TOOLBOX_ARITHMETIC_FUNCTIONS_SIMD_COMMON_ADDS_HPP_INCLUDED
#include <boost/simd/toolbox/arithmetic/functions/adds.hpp>
#include <boost/mpl/logical.hpp>
#include <boost/simd/toolbox/arithmetic/functions/adds.hpp>
#include <boost/simd/include/functions/simd/plus.hpp>
#include <boost/simd/include/functions/simd/bitwise_or.hpp>
#include <boost/simd/include/functions/simd/is_less.hpp>
#include <boost/simd/include/functions/simd/is_greater.hpp>
#include <boost/simd/include/functions/simd/is_gtz.hpp>
#include <boost/simd/include/functions/simd/logical_and.hpp>
#include <boost/simd/include/functions/simd/logical_or.hpp>
#include <boost/simd/include/functions/simd/logical_not.hpp>
//#include <boost/simd/include/functions/simd/logical_notand.hpp>
#include <boost/simd/include/functions/simd/if_else.hpp>
#include <boost/simd/include/functions/simd/min.hpp>
#include <boost/simd/include/functions/simd/max.hpp>
#include <boost/simd/include/constants/valmin.hpp>
#include <boost/simd/include/constants/valmax.hpp>

namespace boost { namespace simd { namespace ext
{
  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::adds_, tag::cpu_,
                          (A0)(X),
                          ((simd_<floating_<A0>,X>))
                          ((simd_<floating_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2) { return a0+a1; }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::adds_, tag::cpu_,
                          (A0)(X),
                          ((simd_<uint_<A0>,X>))
                          ((simd_<uint_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      A0 a0pa1 = a0+a1;
      return if_else(lt(a0pa1, a0), Valmax<A0>(), a0pa1);
    }
  };

  BOOST_SIMD_FUNCTOR_IMPLEMENTATION(boost::simd::tag::adds_, tag::cpu_,
                          (A0)(X),
                          ((simd_<int_<A0>,X>))
                          ((simd_<int_<A0>,X>))
                         )
  {
    typedef A0 result_type;
    BOOST_SIMD_FUNCTOR_CALL_REPEAT(2)
    {
      typedef typename meta::as_logical<A0>::type bA0;
      bA0 gtza0 = is_gtz(a0);
      bA0 gtza1 = is_gtz(a1);
      A0 a0pa1 = a0+a1;
      bA0 test1 = logical_and(logical_and(gtza0, gtza1), lt(a0pa1, a0));
      bA0 test2 = logical_and(logical_not(logical_or(gtza0, gtza1)), gt(a0pa1,a0)); //logical_notand
      return if_else(test1,Valmax<A0>(),if_else(test2,Valmin<A0>(),a0pa1));
    }
  };
} } }
#endif


/******************************************************************************

  This source file is part of the OpenQube project.

  Copyright 2008-2010 Marcus D. Hanwell

  This source code is released under the New BSD License, (the "License").

  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 "slaterset.h"

#ifdef WIN32
#define _USE_MATH_DEFINES
#include <math.h> // needed for M_PI
#endif

#include "cube.h"

//#include <Eigen/Array>
#include <Eigen/Eigenvalues>
#include <Eigen/LU>
#include <Eigen/QR>

#include <cmath>

#include <QtCore/QtConcurrentMap>
#include <QtCore/QFuture>
#include <QtCore/QFutureWatcher>
#include <QtCore/QReadWriteLock>
#include <QtCore/QDebug>

using std::vector;
using Eigen::Vector3d;
using Eigen::Vector3i;
using Eigen::MatrixXd;
using Eigen::SelfAdjointEigenSolver;

namespace OpenQube
{
struct SlaterShell
{
  SlaterSet *set;    // A pointer to the SlaterSet, cannot write to member vars
  Cube *cube;        // The target cube, used to initialise temp cubes too
  unsigned int pos;  // The index of position of the point to calculate the MO for
  unsigned int state;// The MO number to calculate
};

using std::vector;

static const double BOHR_TO_ANGSTROM = 0.529177249;
static const double ANGSTROM_TO_BOHR = 1.0 / 0.529177249;

SlaterSet::SlaterSet() : m_initialized(false)
{
}

SlaterSet::~SlaterSet()
{

}

bool SlaterSet::addAtoms(const std::vector<Eigen::Vector3d> &pos)
{
  m_atomPos = pos;
  return true;
}

bool SlaterSet::addSlaterIndices(const std::vector<int> &i)
{
  m_slaterIndices = i;
  return true;
}

bool SlaterSet::addSlaterTypes(const std::vector<int> &t)
{
  m_slaterTypes = t;
  return true;
}

bool SlaterSet::addZetas(const std::vector<double> &zetas)
{
  m_zetas = zetas;
  return true;
}

bool SlaterSet::addPQNs(const std::vector<int> &pqns)
{
  m_pqns = pqns;
  return true;
}

bool SlaterSet::addOverlapMatrix(const Eigen::MatrixXd &m)
{
  m_overlap.resize(m.rows(), m.cols());
  m_overlap = m;
  return true;
}

bool SlaterSet::addEigenVectors(const Eigen::MatrixXd &e)
{
  m_eigenVectors.resize(e.rows(), e.cols());
  m_eigenVectors = e;
  return true;
}

bool SlaterSet::addDensityMatrix(const Eigen::MatrixXd &d)
{
  m_density.resize(d.rows(), d.cols());
  m_density = d;
  return true;
}

unsigned int SlaterSet::numMOs()
{
  return static_cast<unsigned int>(m_overlap.cols());
}

inline bool SlaterSet::isSmall(double val)
{
  if (val > -1e-15 && val < 1e-15)
    return true;
  else
    return false;
}

unsigned int SlaterSet::factorial(unsigned int n)
{
  if (n <= 1)
    return n;
  return (n * factorial(n-1));
}

void SlaterSet::outputAll()
{

}

bool SlaterSet::calculateCubeMO(Cube *cube, unsigned int state)
{
  // Set up the calculation and ideally use the new QtConcurrent code to
  // multithread the calculation...
  if (state < 1 || static_cast<int>(state) > m_overlap.rows())
    return false;

  if (!m_initialized)
    initialize();

  // It is more efficient to process each shell over the entire cube than it
  // is to process each MO at each point in the cube. This is probably the best
  // point at which to multithread too - QtConcurrent!
  m_slaterShells.resize(static_cast<int>(cube->data()->size()));

  qDebug() << "Number of points:" << m_slaterShells.size();

  for (int i = 0; i < m_slaterShells.size(); ++i) {
    m_slaterShells[i].set = this;
    m_slaterShells[i].cube = cube;
    m_slaterShells[i].pos = i;
    m_slaterShells[i].state = state;
  }

  // Lock the cube until we are done.
  cube->lock()->lockForWrite();

  // Set the cube type
  cube->setCubeType(Cube::MO);

  // Watch for the future
  connect(&m_watcher, SIGNAL(finished()), this, SLOT(calculationComplete()));

  // The main part of the mapped reduced function...
  m_future = QtConcurrent::map(m_slaterShells, SlaterSet::processPoint);
  // Connect our watcher to our future
  m_watcher.setFuture(m_future);

  return true;
}

bool SlaterSet::calculateCubeDensity(Cube *cube)
{
  // Set up the calculation and ideally use the new QtConcurrent code to
  // multithread the calculation...
  if (!m_initialized)
    initialize();

  // It is more efficient to process each shell over the entire cube than it
  // is to process each MO at each point in the cube. This is probably the best
  // point at which to multithread too - QtConcurrent!
  m_slaterShells.resize(static_cast<int>(cube->data()->size()));

  qDebug() << "Number of points for density:" << m_slaterShells.size();

  for (int i = 0; i < m_slaterShells.size(); ++i) {
    m_slaterShells[i].set = this;
    m_slaterShells[i].cube = cube;
    m_slaterShells[i].pos = i;
    m_slaterShells[i].state = 0;
  }

  // Lock the cube until we are done.
  cube->lock()->lockForWrite();

  // Set the cube type
  cube->setCubeType(Cube::ElectronDensity);

  // Watch for the future
  connect(&m_watcher, SIGNAL(finished()), this, SLOT(calculationComplete()));

  // The main part of the mapped reduced function...
  m_future = QtConcurrent::map(m_slaterShells, SlaterSet::processDensity);
  // Connect our watcher to our future
  m_watcher.setFuture(m_future);

  return true;
}

BasisSet * SlaterSet::clone()
{
  SlaterSet *result = new SlaterSet();
  result->m_atomPos = this->m_atomPos;
  result->m_slaterIndices = this->m_slaterIndices;
  result->m_zetas = this->m_zetas;
  result->m_pqns = this->m_pqns;
  result->m_PQNs = this->m_PQNs;

  result->m_factors = this->m_factors;
  result->m_overlap = this->m_overlap;
  result->m_eigenVectors = this->m_eigenVectors;
  result->m_density = this->m_density;
  result->m_normalized = this->m_normalized;
  result->m_initialized = this->m_initialized;

  // Skip tmp variables
  return result;
}

void SlaterSet::calculationComplete()
{
  disconnect(&m_watcher, SIGNAL(finished()), this, SLOT(calculationComplete()));
  qDebug() << m_slaterShells[0].cube->data()->at(0) << m_slaterShells[0].cube->data()->at(1);
  qDebug() << "Calculation complete - cube map...";
  m_slaterShells[0].cube->lock()->unlock();
}

bool SlaterSet::initialize()
{
  m_normalized.resize(m_overlap.cols(), m_overlap.rows());

  SelfAdjointEigenSolver<MatrixXd> s(m_overlap);
  MatrixXd p = s.eigenvectors();
  MatrixXd m = p * s.eigenvalues().array().inverse().array().sqrt()
                    .matrix().asDiagonal() * p.inverse();
  m_normalized = m * m_eigenVectors;

  if (!(m_overlap*m*m).eval().isIdentity())
    qDebug() << "Identity test FAILED - do you need a newer version of Eigen?";
  //    std::cout << m_normalized << std::endl << std::endl;
  //    std::cout << s.eigenvalues() << std::endl << std::endl;
  //    std::cout << m_overlap << std::endl << std::endl;
  //    std::cout << s.eigenvalues().minCoeff() << ' ' << s.eigenvalues().maxCoeff() << std::endl << std::endl;

  m_factors.resize(m_zetas.size());
  m_PQNs = m_pqns;
  // Calculate the normalizations of the orbitals
  for (unsigned int i = 0; i < m_zetas.size(); ++i) {
    switch (m_slaterTypes[i]) {
    case S:
      m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
          sqrt(1.0 / (4.0*M_PI) / factorial(2*m_pqns[i]));
      m_PQNs[i] -= 1;
      break;
    case PX:
    case PY:
    case PZ:
      m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
          sqrt(3.0 / (4.0*M_PI) / factorial(2*m_pqns[i]));
      m_PQNs[i] -= 2;
      break;
    case X2:
      m_factors[i] = 0.5 * pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
          sqrt(15.0 / (4.0*M_PI) / factorial(2*m_pqns[i]));
      m_PQNs[i] -= 3;
      break;
    case XZ:
      m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
          sqrt(15.0 / (4.0*M_PI) / factorial(2*m_pqns[i]));
      m_PQNs[i] -= 3;
      break;
    case Z2:
      m_factors[i] = (0.5/sqrt(3.0)) * pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
          sqrt(15.0 / (4.0*M_PI) / factorial(2*m_pqns[i]));
      m_PQNs[i] -= 3;
      break;
    case YZ:
    case XY:
      m_factors[i] = pow(2.0 * m_zetas[i], m_pqns[i] + 0.5) *
          sqrt(15.0 / (4.0*M_PI) / factorial(2*m_pqns[i]));
      m_PQNs[i] -= 3;
      break;
    default:
      qDebug() << "Orbital" << i << "not handled, type" << m_slaterTypes[i];
    }
  }
  // Convert the exponents into Angstroms
  for (unsigned int i = 0; i < m_zetas.size(); ++i)
    m_zetas[i] = m_zetas[i] / BOHR_TO_ANGSTROM;

  m_initialized = true;

  return true;
}

void SlaterSet::processPoint(SlaterShell &shell)
{
  SlaterSet *set = shell.set;
  unsigned int atomsSize = static_cast<unsigned int>(set->m_atomPos.size());
  unsigned int basisSize = static_cast<unsigned int>(set->m_zetas.size());

  vector<Vector3d> deltas;
  vector<double> dr;
  deltas.reserve(atomsSize);
  dr.reserve(atomsSize);

  // Simply the row of the matrix to operate on
  unsigned int indexMO = shell.state - 1;

  // Calculate our position
  Vector3d pos = shell.cube->position(shell.pos);// * ANGSTROM_TO_BOHR;

  // Calculate the deltas for the position
  for (unsigned int i = 0; i < atomsSize; ++i) {
    deltas.push_back(pos - set->m_atomPos[i]);
    dr.push_back(deltas[i].norm());
  }

  // Now calculate the value at this point in space
  double tmp = 0.0;
  for (unsigned int i = 0; i < basisSize; ++i) {
    tmp += pointSlater(shell.set, deltas[set->m_slaterIndices[i]],
                       dr[set->m_slaterIndices[i]], i, indexMO);
  }
  // Set the value
  shell.cube->setValue(shell.pos, tmp);
}

void SlaterSet::processDensity(SlaterShell &shell)
{
  // Calculate the electron density
  SlaterSet *set = shell.set;
  unsigned int atomsSize = static_cast<unsigned int>(set->m_atomPos.size());
  unsigned int basisSize = static_cast<unsigned int>(set->m_zetas.size());
  unsigned int matrixSize = static_cast<unsigned int>(set->m_density.rows());

  vector<Vector3d> deltas;
  vector<double> dr;
  deltas.reserve(atomsSize);
  dr.reserve(atomsSize);

  // Calculate our position
  Vector3d pos = shell.cube->position(shell.pos);// * ANGSTROM_TO_BOHR;

  // Calculate the deltas for the position
  for (unsigned int i = 0; i < atomsSize; ++i) {
    deltas.push_back(pos - set->m_atomPos[i]);
    dr.push_back(deltas[i].norm());
  }

  // Precompute the factor * exp (-zeta * drs)
  vector<double> expZetas(basisSize);
  for (unsigned int i = 0; i < basisSize; ++i) {
    expZetas[i] = exp(- set->m_zetas[i] * dr[set->m_slaterIndices[i]]);
  }

  // Now calculate the value of the density at this point in space
  double rho = 0.0;
  for (unsigned int i = 0; i < matrixSize; ++i) {
    // Calculate the off-diagonal parts of the matrix
    for (unsigned int j = 0; j < i; ++j) {
      if (isSmall(set->m_density.coeffRef(i, j))) continue;
      double a = 0.0, b = 0.0;
      // Do the first basis
      a = calcSlater(shell.set, deltas[set->m_slaterIndices[i]],
                     dr[set->m_slaterIndices[i]], i);
      b = calcSlater(shell.set, deltas[set->m_slaterIndices[j]],
                     dr[set->m_slaterIndices[j]], j);
      rho += 2.0 * set->m_density.coeffRef(i, j) * (a*b);
    }
    // Now calculate the matrix diagonal
    double tmp = 0.0;
    tmp = calcSlater(shell.set, deltas[set->m_slaterIndices[i]],
                     dr[set->m_slaterIndices[i]], i);
    rho += set->m_density.coeffRef(i, i) * (tmp*tmp);
  }
  // Set the value
  shell.cube->setValue(shell.pos, rho);
}

inline double SlaterSet::pointSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                                     double dr, unsigned int slater,
                                     unsigned int indexMO)
{
  if (isSmall(set->m_normalized.coeffRef(slater, indexMO)))
    return 0.0;
  double tmp = set->m_normalized.coeffRef(slater, indexMO) *
      set->m_factors[slater] * exp(- set->m_zetas[slater] * dr);
  // Radial part with effective PQNs
  for (int i = 0; i < set->m_PQNs[slater]; ++i)
    tmp *= dr;
  switch (set->m_slaterTypes[slater]) {
  case S:
    break;
  case PX:
    tmp *= delta.x();
    break;
  case PY:
    tmp *= delta.y();
    break;
  case PZ:
    tmp *= delta.z();
    break;
  case X2: // (x^2 - y^2)r^n
    tmp *= delta.x() * delta.x() - delta.y() * delta.y();
    break;
  case XZ: // xzr^n
    tmp *= delta.x() * delta.z();
    break;
  case Z2: // (2z^2 - x^2 - y^2)r^n
    tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
        - delta.y() * delta.y();
    break;
  case YZ: // yzr^n
    tmp *= delta.y() * delta.z();
    break;
  case XY: // xyr^n
    tmp *= delta.x() * delta.y();
    break;
  default:
    return 0.0;
  }
  return tmp;
}

inline double SlaterSet::pointSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                                     double dr, unsigned int slater,
                                     unsigned int indexMO, double expZeta)
{
  if (isSmall(set->m_normalized.coeffRef(slater, indexMO)))
    return 0.0;
  double tmp = set->m_normalized.coeffRef(slater, indexMO) * expZeta;
  // Radial part with effective PQNs
  for (int i = 0; i < set->m_PQNs[slater]; ++i)
    tmp *= dr;
  switch (set->m_slaterTypes[slater]) {
  case S:
    break;
  case PX:
    tmp *= delta.x();
    break;
  case PY:
    tmp *= delta.y();
    break;
  case PZ:
    tmp *= delta.z();
    break;
  case X2: // (x^2 - y^2)r^n
    tmp *= delta.x() * delta.x() - delta.y() * delta.y();
    break;
  case XZ: // xzr^n
    tmp *= delta.x() * delta.z();
    break;
  case Z2: // (2z^2 - x^2 - y^2)r^n
    tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
        - delta.y() * delta.y();
    break;
  case YZ: // yzr^n
    tmp *= delta.y() * delta.z();
    break;
  case XY: // xyr^n
    tmp *= delta.x() * delta.y();
    break;
  default:
    return 0.0;
  }
  return tmp;
}

inline double SlaterSet::calcSlater(SlaterSet *set, const Eigen::Vector3d &delta,
                                    double dr, unsigned int slater)
{
  double tmp = set->m_factors[slater] * exp(- set->m_zetas[slater] * dr);
  // Radial part with effective PQNs
  for (int i = 0; i < set->m_PQNs[slater]; ++i)
    tmp *= dr;
  switch (set->m_slaterTypes[slater]) {
  case S:
    break;
  case PX:
    tmp *= delta.x();
    break;
  case PY:
    tmp *= delta.y();
    break;
  case PZ:
    tmp *= delta.z();
    break;
  case X2: // (x^2 - y^2)r^n
    tmp *= delta.x() * delta.x() - delta.y() * delta.y();
    break;
  case XZ: // xzr^n
    tmp *= delta.x() * delta.z();
    break;
  case Z2: // (2z^2 - x^2 - y^2)r^n
    tmp *= 2.0 * delta.z() * delta.z() - delta.x() * delta.x()
        - delta.y() * delta.y();
    break;
  case YZ: // yzr^n
    tmp *= delta.y() * delta.z();
    break;
  case XY: // xyr^n
    tmp *= delta.x() * delta.y();
    break;
  default:
    return 0.0;
  }
  return tmp;
}

}