#pragma once
#ifndef OPENGM_PARTITIONMOVE_HXX
#define OPENGM_PARTITIONMOVE_HXX

#include <algorithm>
#include <vector>
#include <queue>
#include <utility>
#include <string>
#include <iostream>
#include <fstream>
#include <typeinfo>
#include <limits>
#ifdef WITH_BOOST
#include <boost/unordered_map.hpp>
#include <boost/unordered_set.hpp>
#else
#include <ext/hash_map>
#include <ext/hash_set>
#endif

#include "opengm/opengm.hxx"
#include "opengm/inference/inference.hxx"
#include "opengm/inference/new_visitors/new_visitors.hxx"

namespace opengm {

/// \brief Partition Move\n\n
/// Currently Partition Move only implements the Kernighan-Lin-Algorithm
///
/// - Cite: B.W. Kernighan and S. Lin, "An efficent heuristic procedure for partition graphs", 1970
/// - Maximum factor order : second order Potts functions only!
/// - Maximum number of labels : same as the number of variables !
/// - Restrictions : see above
/// - Convergent :   Converge to some local fix point
///
template<class GM, class ACC>
class PartitionMove : public Inference<GM, ACC>
{
public:
   typedef ACC AccumulationType;
   typedef GM GraphicalModelType;
   OPENGM_GM_TYPE_TYPEDEFS;
   typedef size_t LPIndexType;
   typedef visitors::VerboseVisitor<PartitionMove<GM, ACC> > VerboseVisitorType;
   typedef visitors::EmptyVisitor<PartitionMove<GM, ACC> >   EmptyVisitorType;
   typedef visitors::TimingVisitor<PartitionMove<GM, ACC> >  TimingVisitorType;
#ifdef WITH_BOOST
   typedef boost::unordered_map<IndexType, LPIndexType> EdgeMapType;
   typedef boost::unordered_set<IndexType>             VariableSetType;
#else
   typedef __gnu_cxx::hash_map<IndexType, LPIndexType> EdgeMapType;
   typedef __gnu_cxx::hash_set<IndexType>              VariableSetType;
#endif

   struct Parameter{
     Parameter ( ) {};
   };

   ~PartitionMove();
   PartitionMove(const GraphicalModelType&, Parameter para=Parameter());
   virtual std::string name() const {return "PartitionMove";}
   const GraphicalModelType& graphicalModel() const {return gm_;}
   virtual InferenceTermination infer();
   template<class VisitorType> InferenceTermination infer(VisitorType&);
   virtual InferenceTermination arg(std::vector<LabelType>&, const size_t = 1) const;

private:
   enum ProblemType {INVALID, MC, MWC};

   const GraphicalModelType& gm_;
   ProblemType problemType_;
   Parameter parameter_;

   LabelType   numberOfTerminals_;
   LPIndexType numberOfInternalEdges_;

   /// For each variable it contains a map indexed by neighbord nodes giving the index to the LP-variable
   /// e.g. neighbours[a][b] = i means a has the neighbour b and the edge has the index i in the linear objective
   std::vector<EdgeMapType >                       neighbours_;
   std::vector<double>                             edgeWeight_;
   double                                          constant_;
   std::vector<LabelType>                          states_;

   template<class VisitorType> InferenceTermination inferKL(VisitorType&);
   double solveBinaryKL(VariableSetType&, VariableSetType&);

};

template<class GM, class ACC>
PartitionMove<GM, ACC>::~PartitionMove() {
   ;
}

template<class GM, class ACC>
PartitionMove<GM, ACC>::PartitionMove
(
   const GraphicalModelType& gm,
   Parameter para
   ) : gm_(gm), parameter_(para)
{
   if(typeid(ACC) != typeid(opengm::Minimizer) || typeid(OperatorType) != typeid(opengm::Adder)) {
      throw RuntimeError("This implementation does only supports Min-Plus-Semiring.");
   }

   //Set Problem Type
   problemType_           = MC;
   numberOfInternalEdges_ = 0;
   numberOfTerminals_     = gm_.numberOfLabels(0);
   for(size_t i=0; i<gm_.numberOfVariables(); ++i){
      if(gm_.numberOfLabels(i)<gm_.numberOfVariables()) {
         problemType_ = MWC;
         numberOfTerminals_ = std::max(numberOfTerminals_ ,gm_.numberOfLabels(i));
      }
   }
   for(size_t f=0; f<gm_.numberOfFactors();++f) {
      if(gm_[f].numberOfVariables()==0) {
         continue;
      }
      else if(gm_[f].numberOfVariables()==1) {
         problemType_ = MWC;
      }
      else if(gm_[f].numberOfVariables()==2) {
         ++numberOfInternalEdges_;
         if(!gm_[f].isPotts()) {
            problemType_ = INVALID;
            break;
         }
      }
      else{
         problemType_ = INVALID;
         break;
      }
   }

   if(problemType_ == INVALID)
      throw RuntimeError("Invalid Model for Multicut-Solver! Solver requires a potts model!");
   if(problemType_ == MWC)
      throw RuntimeError("Invalid Model for Multicut-Solver! Solver currently do not support first order terms!");

   //Calculate Neighbourhood
   neighbours_.resize(gm_.numberOfVariables());
   edgeWeight_.resize(numberOfInternalEdges_,0);
   constant_=0;
   LPIndexType numberOfInternalEdges=0;
   // Add edges that have to be included
   for(size_t f=0; f<gm_.numberOfFactors(); ++f) {
      if(gm_[f].numberOfVariables()==0) {
         const LabelType l=0;
         constant_+=gm_[f](&l);
      }
      else if(gm_[f].numberOfVariables()==2) {
         LabelType cc0[] = {0,0};
         LabelType cc1[] = {0,1};
         edgeWeight_[numberOfInternalEdges] += gm_[f](cc1) - gm_[f](cc0);
         constant_ += gm_[f](cc0);
         IndexType u = gm_[f].variableIndex(0);
         IndexType v = gm_[f].variableIndex(1);
         neighbours_[u][v] = numberOfInternalEdges;
         neighbours_[v][u] = numberOfInternalEdges;
         ++numberOfInternalEdges;
      }
      else{
         throw RuntimeError("Only supports second order Potts functions!");
      }
   }
   OPENGM_ASSERT(numberOfInternalEdges==numberOfInternalEdges_);

   states_.resize(gm_.numberOfVariables(),0);
   size_t init = 2;

   if(init==1){
      for(size_t i=0; i<states_.size();++i){
         states_[i]=rand()%10;
      }
   }

   if(init==2){
      LabelType p=0;
      std::vector<bool> assigned(states_.size(),false);
      for(IndexType node=0; node<states_.size(); ++node) {
         if(assigned[node])
            continue;
         else{
            std::list<IndexType> nodeList;
            states_[node]  = p;
            assigned[node] = true;
            nodeList.push_back(node);
            while(!nodeList.empty()) {
               size_t n=nodeList.front(); nodeList.pop_front();
               for(typename EdgeMapType::const_iterator it=neighbours_[n].begin() ; it != neighbours_[n].end(); ++it) {
                  const IndexType node2 = (*it).first;
                  if(!assigned[node2] && edgeWeight_[(*it).second]>0) {
                     states_[node2] = p;
                     assigned[node2] = true;
                     nodeList.push_back(node2);
                  }
               }
            }
            ++p;
         }
      }
   }

   if(init==3){
      for(size_t i=0; i<states_.size();++i){
         states_[i]=i;
      }
   }

}

template <class GM, class ACC>
InferenceTermination
PartitionMove<GM,ACC>::infer()
{
   EmptyVisitorType visitor;
   return infer(visitor);
}

template <class GM, class ACC>
template<class VisitorType>
InferenceTermination
PartitionMove<GM,ACC>::infer(VisitorType& visitor)
{
   visitor.begin(*this);
   inferKL(visitor);
   visitor.end(*this);
   return NORMAL;
}

template <class GM, class ACC>
template<class VisitorType>
InferenceTermination
PartitionMove<GM,ACC>::inferKL(VisitorType& visitor)
{
   // Current Partition-Sets
   std::vector<VariableSetType> partitionSets;

   // Set-Up Partition-Sets from current/initial partitioning
   LabelType numberOfPartitions =0;
   for(size_t i=0; i<states_.size(); ++i)
      if(states_[i]+1>numberOfPartitions) numberOfPartitions=states_[i]+1;
   partitionSets.resize(numberOfPartitions);
   for(IndexType i=0; i<states_.size(); ++i){
      partitionSets[states_[i]].insert(i);
   }

   bool change = true;
   while(change){
      // std::cout << numberOfPartitions << " conncted subsets."<<std::endl;
      change = false;
      std::vector<size_t> pruneSets;
      // Check all pairs of partitions
      for(size_t part0=0; part0<numberOfPartitions; ++part0){
         //std::cout <<"*"<<std::flush;
         // Find neighbord sets
         std::set<size_t> neighbordSets;
         for(typename VariableSetType::const_iterator it=partitionSets[part0].begin(); it!=partitionSets[part0].end(); ++it){
            const IndexType node = (*it);
            for(typename EdgeMapType::const_iterator nit=neighbours_[node].begin() ; nit != neighbours_[node].end(); ++nit) {
                 const IndexType node2 = (*nit).first;
                 if(states_[node2]>part0){
                    neighbordSets.insert(states_[node2]);
                 }
            }
         }
         for(std::set<size_t>::const_iterator it=neighbordSets.begin(); it!=neighbordSets.end();++it){
            size_t part1 = *it;
            //for(size_t part1=part0+1; part1<numberOfPartitions; ++part1){
            if(partitionSets[part0].size()==0 || partitionSets[part1].size()==0)
               continue;
            double improvement = solveBinaryKL(partitionSets[part0],partitionSets[part1]);
            //std::cout <<part0<<" vs "<<part1<<" : " <<improvement<<std::endl;
            OPENGM_ASSERT(improvement<1e-8);
            if(-1e-8>improvement){
               change = true; // Partition has been improved
            }
         }
      }
      // Check for each Partition ...
      for(size_t part0=0; part0<numberOfPartitions; ++part0){
         // ... if it is empty and can be pruned
         if(partitionSets[part0].size()==0){
            //std::cout <<"Remove "<<part0<<std::endl;
            pruneSets.push_back(part0);
         }
         // ... or if it can be splited into two sets
         else if(partitionSets[part0].size()>1){
            // std::cout <<part0<<" vs "<<"NULL"<<std::endl;

            VariableSetType emptySet(partitionSets[part0].size());
            double improvement = solveBinaryKL(partitionSets[part0], emptySet);
            if(emptySet.size()>0){
               OPENGM_ASSERT(improvement<0);
               partitionSets.push_back(emptySet);
               change = true;
            }
         }
      }
      // Remove sets marked as to prune
      //std::cout << "Remove " <<pruneSets.size() << " subsets."<<std::endl;
      for(size_t i=0; i<pruneSets.size(); ++i){
         size_t part = pruneSets[pruneSets.size()-1-i];
         partitionSets.erase( partitionSets.begin()+part);
      }
      // Update Labeling
      numberOfPartitions = partitionSets.size();
      for(size_t part=0; part<numberOfPartitions; ++part){
         for(typename VariableSetType::const_iterator it=partitionSets[part].begin(); it!=partitionSets[part].end(); ++it){
            states_[*it] = part;
         }
      }
      if( visitor(*this) != visitors::VisitorReturnFlag::ContinueInf ){
         change = false;
      }
   }
   return NORMAL;
}

template <class GM, class ACC>
double PartitionMove<GM,ACC>::solveBinaryKL
(
   VariableSetType& set0,
   VariableSetType& set1
)
{
   double improvement = 0.0;
   //std::cout << "Set0: "<< set0.size() <<" Set1: "<< set1.size() << std::endl;

   for(size_t outerIt=0; outerIt<100;++outerIt){
      // Compute D[n] = E_n - I_n
      std::vector<double> D(gm_.numberOfVariables(),0);
      for(typename VariableSetType::const_iterator it=set0.begin(); it!=set0.end(); ++it){
         double E_a = 0.0;
         double I_a = 0.0;
         const IndexType node = *it;
         for (typename EdgeMapType::const_iterator eit=neighbours_[node].begin(); eit!=neighbours_[node].end(); ++eit){
            const IndexType node2 = (*eit).first;
            const double weight = edgeWeight_[(*eit).second];

            if (set0.find(node2) != set0.end()) {
                I_a += weight;
            }
            else if(set1.find(node2) != set1.end()){
               E_a += weight;
            }
         }
         D[node] = -(E_a - I_a);
      }
      for(typename VariableSetType::const_iterator it=set1.begin(); it!=set1.end(); ++it){
         double E_a = 0.0;
         double I_a = 0.0;
         const IndexType node = *it;
         for(typename EdgeMapType::const_iterator eit=neighbours_[node].begin(); eit!=neighbours_[node].end(); ++eit){
            const IndexType node2 = (*eit).first;
            const double weight = edgeWeight_[(*eit).second];

            if (set1.find(node2) != set1.end()) {
                I_a += weight;
            }
            else if(set0.find(node2) != set0.end()){
               E_a += weight;
            }
         }
         D[node] = -(E_a - I_a);
      }

      double d=0;
      for(size_t i=0; i<D.size(); ++i){
         if(D[i]<d)
            d=D[i];
      }

      // Search a gready move sequence
      std::vector<bool>      isMovedNode(gm_.numberOfVariables(),false);
      std::vector<IndexType> nodeSequence;
      std::vector<double>    improveSequence;
      std::vector<double>    improveSumSequence(1,0.0);
      size_t                 bestMove=0;

      // Build sequence of greedy best moves
      for(size_t innerIt=0; innerIt<1000; ++innerIt){
         double    improve = std::numeric_limits<double>::infinity();
         IndexType node;
         bool      moved = false;
         // Search over moves from set0
         for(typename VariableSetType::const_iterator it=set0.begin(); it!=set0.end(); ++it){
            if(isMovedNode[*it]){
               continue;
            }
            else{
               if(D[*it]<improve){
                  improve = D[*it];
                  node    = *it;
                  moved   = true;
               }
            }
         }
         // Search over moves from set1
         for(typename VariableSetType::const_iterator it=set1.begin(); it!=set1.end(); ++it){
            if(isMovedNode[*it]){
               continue;
            }
            else{
               if(D[*it]<improve){
                  improve = D[*it];
                  node    = *it;
                  moved   = true;
               }
            }
         }

         // No more moves?
         if(moved == false){
            break;
         }

         // Move node and recalculate D
         //std::cout << " " <<improveSumSequence.back()+improve;
         isMovedNode[node]=true;
         nodeSequence.push_back(node);
         improveSumSequence.push_back(improveSumSequence.back()+improve);
         improveSequence.push_back(improve);
         if (improveSumSequence[bestMove]>improveSumSequence.back()) {
            bestMove = improveSumSequence.size()-1;
         }

         VariableSetType& mySet = set0.find(node) != set0.end() ? set0 : set1;
         for(typename EdgeMapType::const_iterator eit=neighbours_[node].begin(); eit!=neighbours_[node].end(); ++eit){
            IndexType node2  = (*eit).first;
            double    weight = edgeWeight_[(*eit).second];
            if(mySet.find(node2) != mySet.end()){
               D[node2] -= 2.0 * weight;
            }
            else{
               D[node2] += 2.0 * weight;
            }

         }
      }

       // Perform Move
      if(improveSumSequence[bestMove]>-1e-10)
         break;
      else{
         improvement += improveSumSequence[bestMove];
         for (size_t i = 0; i < bestMove; ++i) {
            int node = nodeSequence[i];
            if (set0.find(node) != set0.end()) {
               set0.erase(node);
               set1.insert(node);
            }
            else{
               set1.erase(node);
               set0.insert(node);
            }
         }
      }
      // Search for the next move if this move has give improvement
   }
   return improvement;
}

template <class GM, class ACC>
InferenceTermination
PartitionMove<GM,ACC>::arg
(
   std::vector<typename PartitionMove<GM,ACC>::LabelType>& x,
   const size_t N
   ) const
{
   if(N!=1) {
      return UNKNOWN;
   }
   else{
      x.resize(gm_.numberOfVariables());
      for(size_t i=0; i<gm_.numberOfVariables(); ++i)
         x[i] = states_[i];
      return NORMAL;
   }
}

} // end namespace opengm

#endif


/*!@file
 * @copyright This code is licensed under the 3-clause BSD license.
 *   Copyright ETH Zurich, Laboratory of Physical Chemistry, Reiher Group.
 *   See LICENSE.txt for details.
 */

#include "Molassembler/DistanceGeometry/TetrangleSmoothing.h"
#include "Molassembler/DistanceGeometry/ValueBounds.h"
#include "Molassembler/Temple/constexpr/Array.h"
#include "Molassembler/Temple/constexpr/Numeric.h"
#include "Molassembler/Temple/Invoke.h"

#include <Eigen/Dense>
#include <cfenv>

namespace Scine {
namespace Molassembler {
namespace DistanceGeometry {

struct LU {
  Eigen::Matrix4d L = Eigen::Matrix4d::Zero();
  Eigen::Matrix4d U = Eigen::Matrix4d::Zero();

  LU(
    const Eigen::MatrixXd& bounds,
    const std::array<unsigned, 4>& indices
  ) {
    for(unsigned i = 0; i < 4; ++i) {
      for(unsigned j = i + 1; j < 4; ++j) {
        unsigned a = indices[i];
        unsigned b = indices[j];
        if(a > b) {
          std::swap(a, b);
        }

        U(i, j) = bounds(a, b);
        U(j, i) = bounds(a, b);
        L(i, j) = bounds(b, a);
        L(j, i) = bounds(b, a);
      }
    }
  }

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

// Named parameters to indicate when indices passed to lower and upper are ordered
struct OrderedIndicesTag {};
constexpr OrderedIndicesTag orderedIndicesTag;

constexpr unsigned pickMissingInZeroToThree(unsigned a, unsigned b, unsigned c) {
  if(
    !(a < 4 && b < 4 && c < 4)
    || a == b
    || a == c
    || b == c
  ) {
    throw std::logic_error("Unsafe arguments!");
  }

  unsigned x = 0;

  for(unsigned i = 0; i < 4; ++i) {
    if(a != i && b != i && c != i) {
      x = i;
      break;
    }
  }

  return x;
}

struct TriCheckResult {
  /* Lower and upper bounds on the 3-4/k-l distance as per triangle
   * inequalities
   */
  double klLowerBound; // l3_lim
  double klUpperBound; // u3_lim

  /* Whether or not the various triangle limits can be attained without
   * violating any of the passed bounds.
   */
  bool l_col = false;
  bool u_col = false;
};

template<typename MatrixType, typename ... AssumeISmallerJPack>
std::conditional_t<
  std::is_const<std::remove_reference_t<MatrixType>>::value,
  double,
  double&
> upper(MatrixType&& matrix, const unsigned i, const unsigned j, AssumeISmallerJPack ... pack) {
  if(sizeof...(pack) > 0) {
    return matrix(i, j);
  }

  if(i < j) {
    return matrix(i, j);
  }

  return matrix(j, i);
}

template<typename MatrixType, typename ... AssumeISmallerJPack>
std::conditional_t<
  std::is_const<std::remove_reference_t<MatrixType>>::value,
  double,
  double&
> lower(MatrixType&& matrix, const unsigned i, const unsigned j, AssumeISmallerJPack ... pack) {
  if(sizeof...(pack) > 0) {
    return matrix(j, i);
  }

  if(i < j) {
    return matrix(j, i);
  }

  return matrix(i, j);
}

double T(
  const double d_pr,
  const double d_qr,
  const double d_ps,
  const double d_qs
) {
  if(d_pr + d_qr > d_ps + d_qs || d_pr + d_qr < std::fabs(d_ps - d_qs)) {
    throw std::logic_error("T preconditions violated!");
  }

  return (
    d_pr * (std::pow(d_qs, 2) - std::pow(d_qr, 2))
    + d_qr * (std::pow(d_ps, 2) - std::pow(d_pr, 2))
  ) / (d_pr + d_qr);
}

bool collinear(
  const double d_pr,
  const double d_qr,
  const double l_ps,
  const double l_qs,
  const double l_rs,
  const double u_ps,
  const double u_qs,
  const double u_rs
) {
  auto U = [&]() -> double {
    if(d_pr + d_qr < u_ps - u_qs) {
      return std::pow(d_qr + u_qs, 2);
    }

    if(d_pr + d_qr < u_qs - u_ps) {
      return std::pow(d_pr + u_ps, 2);
    }

    return T(d_pr, d_qr, u_ps, u_qs);
  };

  auto L = [&]() -> double {
    if(d_pr + d_qr > l_ps + l_qs) {
      return std::pow(
        std::max({
          0.0,
          d_qr - u_qs,
          d_pr - u_ps,
          l_qs - d_qr,
          l_ps - d_pr
        }),
        2
      );
    }

    if(d_pr + d_qr < l_ps - l_qs) {
      return std::pow(l_ps - d_pr, 2);
    }

    if(d_pr + d_qr < l_qs - l_ps) {
      return std::pow(l_qs - d_qr, 2);
    }

    return T(d_pr, d_qr, l_ps, l_qs);
  };

  return (
    d_pr + d_qr <= u_ps + u_qs
    && u_rs >= L()
    && l_rs <= U()
  );
}

[[gnu::pure]] constexpr unsigned decrement(unsigned i) {
  if(i == 0) {
    throw std::logic_error("Underflow");
  }

  return i - 1;
}

/* limitTest overload set definitions section */

template<bool isUpper, unsigned i, unsigned j, unsigned k>
std::enable_if_t<isUpper, bool> limitTest(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper,
  const double minimalUpperLimit,
  const double tripletLimit
) {
  // Triangle upper limit test
  static_assert(i == 2 && k == 3, "Unexpected instantiation!");
  constexpr unsigned l = pickMissingInZeroToThree(i, j, k);
  static_assert(j < 4 && l < 4, "Zero-based indices");

  return (
    tripletLimit == minimalUpperLimit
    && collinear(
      upper(i, decrement(3)),
      upper(i, decrement(4)),
      lower(j, decrement(3)),
      lower(j, decrement(4)),
      lower(i, j),
      upper(j, decrement(3)),
      upper(j, decrement(4)),
      upper(i, j)
    )
  );
}

template<bool isUpper, unsigned i, unsigned k, unsigned l>
std::enable_if_t<!isUpper, bool> limitTest(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper,
  const double maximalLowerLimit,
  const double tripletLimit
) {
  // Triangle lower limit test
  constexpr unsigned j = pickMissingInZeroToThree(i, k, l);
  return(
    tripletLimit == maximalLowerLimit
    && collinear(
      upper(i, k),
      /* Paper has l_3(b_k, b_l) below, assuming that's a typo, as it would
       * mean the lower triangle inequality bound between k and l instead
       * of the current lower bound on that quantity. There is no such
       * subscripted quantity in the place of the paper where triangle upper
       * limit tests are performed.
       */
      lower(k, l),
      lower(i, j),
      lower(j, l),
      lower(j, k),
      upper(i, j),
      upper(j, l),
      upper(j, k)
    )
  );
}

template<bool isUpper, unsigned k, unsigned i, unsigned j, unsigned l>
std::enable_if_t<isUpper, bool> limitTest(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper,
  const double minimalUpperLimit,
  const double quadrupletLimit
) {
  // Tetrangle upper limit test
  if(quadrupletLimit == minimalUpperLimit) {
    const double firstConditionA = upper(j, k);
    const double firstConditionB = (
      upper(i, k) + upper(i, j)
    );
    const double firstConditionC = lower(j, k);

    // ujk >= uik + uij >= ljk
    if(firstConditionA >= firstConditionB && firstConditionB >= firstConditionC) {
      const double secondConditionA = upper(i, l);
      const double secondConditionB = (
        upper(j, l) + upper(i, l)
      );
      const double secondConditionC = lower(i, l);

      // uil >= ujl + uil >= lil
      return (secondConditionA >= secondConditionB && secondConditionB >= secondConditionC);
    }

    return false;
  }

  return false;
}

// First set of 4-atom lower limits with L[i, k, l, j] with {k, l} = {3, 4}
template<bool isUpper, unsigned i, unsigned k, unsigned l, unsigned j>
std::enable_if_t<
  !isUpper && (
    (k == 2 && l == 3) || (k == 3 && l == 2)
  ),
  bool
> limitTest(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper,
  const double maximalLowerLimit,
  const double quadrupletLimit
) {
  if(quadrupletLimit == maximalLowerLimit) {
    const double firstConditionA = upper(i, k);
    const double firstConditionB = lower(i, j) - upper(i, k);
    const double firstConditionC = lower(j, k);

    // uik >= lij - uik >= ljk
    if(firstConditionA >= firstConditionB && firstConditionB >= firstConditionC) {
      const double secondConditionA = upper(i, l);
      const double secondConditionB = lower(i, j) - upper(j, l);
      const double secondConditionC = lower(i, l);

      // uil >= lij - ujl >= lil
      return (secondConditionA >= secondConditionB && secondConditionB >= secondConditionC);
    }

    return false;
  }

  return false;
}

/* Second set of 4-atom lower limits with L[i, j, k, l] with {k, l} = {3, 4}
 * Here, k and l are the last two indices instead of in the middle.
 */
template<bool isUpper, unsigned i, unsigned j, unsigned k, unsigned l>
std::enable_if_t<
  !isUpper && (
    (k == 2 && l == 3) || (k == 3 && l == 2)
  ),
  bool
> limitTest(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper,
  const double maximalLowerLimit,
  const double quadrupletLimit
) {
  if(quadrupletLimit == maximalLowerLimit) {
    const double firstConditionA = upper(j, l);
    const double firstConditionB = lower(i, l) - upper(i, j);
    const double firstConditionC = lower(j, l);

    // ujl >= lil - uij >= ljl
    if(firstConditionA >= firstConditionB && firstConditionB >= firstConditionC) {
      const double secondConditionA = upper(i, k);
      const double secondConditionB = upper(i, j) + upper(j, k);
      const double secondConditionC = lower(i, k);

      // uik >= uij + ujk >= lik
      return (secondConditionA >= secondConditionB && secondConditionB >= secondConditionC);
    }

    return false;
  }

  return false;
}

bool triangleInequalitiesHold(const Eigen::Matrix3d& matrix) {
  // Ensure triangle inequalities are satisified in this matrix
  for(unsigned k = 0; k < 3; ++k) {
    for(unsigned i = 0; i < 3; ++i) {
      for(unsigned j = i + 1; j < 3; ++j) {
        if(upper(matrix, i, j, orderedIndicesTag) > upper(matrix, i, k) + upper(matrix, k, j)) {
          return false;
        }

        if(lower(matrix, i, j, orderedIndicesTag) < lower(matrix, i, k) - upper(matrix, k, j)) {
          return false;
        }
      }
    }
  }

  return true;
}

bool zeroBoundTest(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper,
  const double maximalLowerLimit
) {
  auto l = [&lower](unsigned i, unsigned j) {
    return lower(decrement(i), decrement(j));
  };
  auto u = [&upper](unsigned i, unsigned j) {
    return upper(decrement(i), decrement(j));
  };

  if(maximalLowerLimit == 0.0) {
    Eigen::Matrix3d c;

    // if i < j, then i,j an upper bound, i > j, a lower bound
    c(1, 0) = l(1, 2);
    c(0, 1) = u(1, 2);
    c(2, 0) = std::max(l(1, 3), l(1, 4));
    c(0, 2) = std::min(u(1, 3), u(1, 4));
    c(2, 1) = std::max(l(2, 3), l(2, 4));
    c(1, 2) = std::min(u(2, 3), u(2, 4));

    return triangleInequalitiesHold(c);
  }

  return false;
}

/* This function is a catch-all for the various bounds calculated in the
 * beginning of triCheck based on an index_sequence type argument.
 *
 * Although it may seem ridiculous to instantiate this for each index sequence
 * if the matrix element access is resolved at runtime anyway, using
 * index_sequence to store the various sequences allows for code reuse in the
 * checking stage of triCheck. It also allows us to group the expressions
 * for the various bounds nicely.
 *
 * Note that this takes zero-based indices! Make sure to use decrement() for
 * the index_sequence with which this is instantiated.
 */
constexpr unsigned NAngleBoundNoneValue = 100;
template<bool isUpper, bool firstPattern, unsigned i, unsigned j, unsigned k, unsigned l = NAngleBoundNoneValue>
double NAngleBound(const Eigen::Matrix4d& lower, const Eigen::Matrix4d& upper) {
  static_assert(i < 4 && j < 4 && k < 4, "Zero-based indices!");
  static_assert(l == NAngleBoundNoneValue || l < 4, "Optional d argument is not zero-based!");

  // NOTE all ifs here could be if constexpr in C++17
  if(l == NAngleBoundNoneValue) {
    // Triangle algorithms
    if(isUpper) {
      // Upper triangle bound
      return upper(i, j) + upper(j, k);
    }

    // Lower triangle bound
    return lower(i, k) - upper(i, j);
  }

  // Tetrangle algorithms: l != NAngleBoundNoneValue
  if(isUpper) {
    // Upper tetrangle bound
    return upper(i, j) + upper(j, k) + upper(k, l);
  }

  // Lower tetrangle bound (THESE ARE IRREGULAR!)
  if(firstPattern) {
    return lower(i, l) - upper(i, j) - upper(k, l); // for the first two
  }

  return lower(i, l) - upper(i, j) - upper(j, k); // for the last four
}

template<bool isUpper, bool firstPattern, std::size_t ... Inds>
double NAngleBoundsMap(const Eigen::Matrix4d& lower, const Eigen::Matrix4d& upper, std::index_sequence<Inds ...> /* inds */) {
  // Raise the index sequence from function argument to template argument
  return NAngleBound<isUpper, firstPattern, decrement(Inds)...>(lower, upper);
}

template<typename TupleType, bool isUpper, bool firstPattern, std::size_t ... TupleInds>
auto mapIndexSetsHelper(const Eigen::Matrix4d& lower, const Eigen::Matrix4d& upper, std::index_sequence<TupleInds ...> /* inds */) {
  // Create an array with the results of bounds evaluations
  return std::array<double, sizeof...(TupleInds)> {
    NAngleBoundsMap<isUpper, firstPattern>(
      lower,
      upper,
      std::tuple_element_t<TupleInds, TupleType> {}
    )...
  };
}

/* isUpper denotes whether we are calculating an upper or lower bound
 * firstPattern is only for the tetrangle lower limits, where there are two
 * index patterns.
 */
template<typename TupleType, bool isUpper, bool firstPattern>
auto mapIndexSets(const Eigen::Matrix4d& lower, const Eigen::Matrix4d& upper) {
  // Enumerate the index sequences in TupleType
  return mapIndexSetsHelper<TupleType, isUpper, firstPattern>(
    lower,
    upper,
    std::make_index_sequence<std::tuple_size<TupleType>::value> {}
  );
}

template<typename ArgsTuple, typename CallableArgPair>
bool foldCallableArgsPairLogicalOr(
  const ArgsTuple& args,
  CallableArgPair&& a
) {
  /* Piece together the arguments in order to call LimitTester::operator(),
   * which forwards its arguments to the limitTest overload set
   *
   * The CallableArgPair is created in limitAnyOfHelper and consists of a
   * LimitTester and a part of the arguments needed to call it.
   */
  return Temple::invoke(a.first, std::tuple_cat(args, std::tie(a.second)));
}

// C++17 fold
template<typename ArgsTuple, typename CallableArgPair, typename OtherCallableArgPair, typename ... Conditionals>
bool foldCallableArgsPairLogicalOr(
  const ArgsTuple& args,
  CallableArgPair&& a,
  OtherCallableArgPair&& b,
  Conditionals ... conditionals
) {
  // Fold the results of calls hopefully with short-circuiting
  return (
    foldCallableArgsPairLogicalOr(args, a)
    || foldCallableArgsPairLogicalOr(args, b, conditionals...)
  );
}

template<bool isUpper, std::size_t ... Inds>
struct LimitTester {
  template<typename ... Args>
  bool operator() (Args&& ... args) {
    return limitTest<isUpper, decrement(Inds)...>(std::forward<Args>(args)...);
  }
};

template<bool isUpper, std::size_t ... Inds>
auto makeLimitTester(std::index_sequence<Inds ...> /* inds */) {
  // Raise index sequence from function argument to template argument
  return LimitTester<isUpper, Inds ...> {};
}

template<typename TupleType, bool isUpper, typename ArgsTuple, std::size_t ... Inds>
auto limitAnyOfHelper(
  const ArgsTuple& args,
  const std::array<double, std::tuple_size<TupleType>::value>& values,
  std::index_sequence<Inds ...> /* inds */
) {
  /* Create a long list of pairs of test functors corresponding to index
   * sequences in TupleType that we can fold with logical or
   */
  return foldCallableArgsPairLogicalOr(
    args,
    std::make_pair(
      makeLimitTester<isUpper>(std::tuple_element_t<Inds, TupleType> {}),
      values.at(Inds)
    )...
  );
}

template<typename TupleType, bool isUpper, typename ArgsTuple>
auto limitAnyOf(
  const ArgsTuple& args,
  const std::array<double, std::tuple_size<TupleType>::value>& values
) {
  // Enumerate the indices into TupleType
  return limitAnyOfHelper<TupleType, isUpper>(
    args,
    values,
    std::make_index_sequence<std::tuple_size<TupleType>::value> {}
  );
}

TriCheckResult triCheck(
  const Eigen::Matrix4d& lower,
  const Eigen::Matrix4d& upper
) {
  TriCheckResult result;
  /* The comments here sometimes refer to the parts of TRI_CHECK that they
   * implement from the original algorithm.
   *
   * Here we first define all the index sequences we will need as types so we
   * can freely mess with them. Note that these are one-based purely to match
   * the definitions from the original algorithm description.
   */
  // Definitions of upper triangle index sets that `U` is called with
  using UpperTriangleIndexSets = std::tuple<
    std::index_sequence<3, 1, 4>,
    std::index_sequence<3, 2, 4>
  >;

  // Definitions of upper tetrangle index sets that `U` is called with
  using UpperTetrangleIndexSets = std::tuple<
    std::index_sequence<3, 1, 2, 4>,
    std::index_sequence<4, 1, 2, 3>
  >;

  // Definitions of lower triangle index sets that `L` is called with
  using LowerTriangleIndexSets = std::tuple<
    std::index_sequence<1, 3, 4>,
    std::index_sequence<2, 3, 4>,
    std::index_sequence<1, 4, 3>,
    std::index_sequence<2, 4, 3>
  >;

  // Lower tetrangle index sets following the pattern l_ik - u_ij - u_kl
  using FirstLowerTetrangleIndexSets = std::tuple<
    std::index_sequence<1, 3, 4, 2>,
    std::index_sequence<1, 4, 3, 2>
  >;

  // Lower tetrangle index sets following the pattern l_ik - u_ij - u_jk
  using SecondLowerTetrangleIndexSets = std::tuple<
    std::index_sequence<1, 2, 3, 4>,
    std::index_sequence<2, 1, 3, 4>,
    std::index_sequence<1, 2, 4, 3>,
    std::index_sequence<2, 1, 4, 3>
  >;

  /* Now we actually calculate all of those values by calling `U` and `L` with
   * them. mapIndexSets takes care of forwarding the index sequences to the
   * right functions and grouping the results into arrays. The first boolean
   * template argument denotes whether we are calculating an upper bound, which
   * we are.
   *
   * The second is relevant only to the lower tetrangle bounds, and its value
   * is just set false here.
   *
   * The three- and four-argument `L` and `U` functions are represented by
   * the function NAngleBound.
   */
  const auto upperTriangleBounds = mapIndexSets<UpperTriangleIndexSets, true, false>(lower, upper);
  const auto upperTetrangleBounds = mapIndexSets<UpperTetrangleIndexSets, true, false>(lower, upper);

  /* Now for the lower bounds. Regarding the tetrangle bounds, you saw in the
   * index sequence definition that there are two sets, each following a
   * different pattern. If the second boolean template argument to mapIndexSets
   * is true, that means the current set follows the first pattern.
   */
  const auto lowerTriangleBounds = mapIndexSets<LowerTriangleIndexSets, false, false>(lower, upper);
  const auto firstLowerTetrangleBounds = mapIndexSets<FirstLowerTetrangleIndexSets, false, true>(lower, upper);
  const auto secondLowerTetrangleBounds = mapIndexSets<SecondLowerTetrangleIndexSets, false, false>(lower, upper);

  /* We set u3 and l3 from the minimum and maximum of the calculated bounds */
  result.klUpperBound = std::min(
    Temple::min(upperTriangleBounds), // min(U[i, j, k])
    Temple::min(upperTetrangleBounds) // min(U[i, j, k, l])
  );

  result.klLowerBound = std::max({
    0.0, // L[0] := 0
    Temple::max(lowerTriangleBounds), // min(L[i, j, k])
    Temple::max(firstLowerTetrangleBounds), // min(L[i, j, k, l]) of first pattern
    Temple::max(secondLowerTetrangleBounds) // min(L[i, j, k, l]) of second pattern
  });

  /* Now we determine u_col and l_col. These are "true whenever the triangle
   * inequality limits on the (3, 4)-distance are attainable without violating
   * the given bounds or the tetrangle inequality.
   *
   * The algorithm itself has multiple "For each 3-atom upper limit", "For each
   * 4-atom upper limit" parts that we abstract over here. If any of those
   * trigger, then the rest need not be evaluated, so we short-circuit with
   * logical ors.
   */

  const auto upperArgs = std::tie(lower, upper, result.klUpperBound);
  result.u_col = (
    limitAnyOf<UpperTriangleIndexSets, true>(upperArgs, upperTriangleBounds)
    || limitAnyOf<UpperTetrangleIndexSets, true>(upperArgs, upperTetrangleBounds)
  );

  const auto lowerArgs = std::tie(lower, upper, result.klLowerBound);
  result.l_col = (
    Temple::invoke(zeroBoundTest, lowerArgs)
    || limitAnyOf<LowerTriangleIndexSets, false>(lowerArgs, lowerTriangleBounds)
    || limitAnyOf<FirstLowerTetrangleIndexSets, false>(lowerArgs, firstLowerTetrangleBounds)
    || limitAnyOf<SecondLowerTetrangleIndexSets, false>(lowerArgs, secondLowerTetrangleBounds)
  );

  return result;
}

double CMUpper(
  const double d12,
  const double d13,
  const double d14,
  const double d23,
  const double d24
) {
  const double a = std::pow(d12, 2);
  const double b = std::pow(d13, 2);
  const double c = std::pow(d14, 2);
  const double d = std::pow(d23, 2);
  const double e = std::pow(d24, 2);

  const double A = -a / 4;
  const double B = (
    a * (-a + b + c + d + e)
    - b * c + b * e + c * d - d * e
  ) / 4;
  const double C = (
    d * (- a * b + a * c + b * c - c * c - c * d)
    + e * (a * b - b * b - a * c + b * c + b * d + c * d - b * e)
  ) / 4;

  const double discriminant = B * B + a * C; // A = -a / 4 -> -4 A = a

  if(discriminant >= 0) {
    return (-B - std::sqrt(discriminant)) / (2.0 * A);
  }

  return std::numeric_limits<double>::lowest();
}

double CMLower(
  const double d12,
  const double d13,
  const double d14,
  const double d23,
  const double d24
) {
  // NOTE: Nearly identical to CMUpper
  const double a = std::pow(d12, 2);
  const double b = std::pow(d13, 2);
  const double c = std::pow(d14, 2);
  const double d = std::pow(d23, 2);
  const double e = std::pow(d24, 2);

  const double A = -a / 4;
  const double B = (
    a * (-a + b + c + d + e)
    - b * c + b * e + c * d - d * e
  ) / 4;
  const double C = (
    d * (- a * b + a * c + b * c - c * c - c * d)
    + e * (a * b - b * b - a * c + b * c + b * d + c * d - b * e)
  ) / 4;

  const double discriminant = B * B + a * C; // A = -a / 4 -> -4 A = a

  if(discriminant >= 0) {
    return (-B + std::sqrt(discriminant)) / (2.0 * A);
  }

  return std::numeric_limits<double>::max();
}

/* Namespace with non-templated lower and upper functions so they can be
 * template parameters for other functions
 */
namespace m {

template<typename FnTuple, std::size_t ... Inds>
auto makeTetrangleCallTupleHelper(
  const Eigen::MatrixXd& bounds,
  const std::array<unsigned, 4>& b,
  FnTuple fnTuple,
  std::index_sequence<Inds...> /* inds */
) {
  constexpr auto indexTuple = std::make_tuple(
    std::pair<unsigned, unsigned> {0, 1},
    std::pair<unsigned, unsigned> {0, 2},
    std::pair<unsigned, unsigned> {0, 3},
    std::pair<unsigned, unsigned> {1, 2},
    std::pair<unsigned, unsigned> {1, 3}
  );

  return std::make_tuple(
    std::get<Inds>(fnTuple)(
      bounds,
      b[std::get<Inds>(indexTuple).first],
      b[std::get<Inds>(indexTuple).second]
    )...
  );
}

template<typename ... Fns>
auto fetchAppropriateBounds(
  const Eigen::MatrixXd& bounds,
  const std::array<unsigned, 4>& b,
  Fns ... fns
) {
  return makeTetrangleCallTupleHelper(
    bounds,
    b,
    std::forward_as_tuple(fns...),
    std::make_index_sequence<sizeof...(fns)>()
  );
}

double l(const Eigen::MatrixXd& bounds, const unsigned i, const unsigned j) {
  if(i < j) {
    return bounds(j, i);
  }

  return bounds(i, j);
}

double u(const Eigen::MatrixXd& bounds, const unsigned i, const unsigned j) {
  if(i < j) {
    return bounds(i, j);
  }

  return bounds(j, i);
}

} // namespace m

double upperTetrangleLimit(
  const Eigen::MatrixXd& bounds,
  const std::array<unsigned, 4>& b
) {
  /* Indices are always the following sequence: 12, 13, 14, 23, 24.
   * The only thing different among the tetrangle limits is whether we pass
   * the lower or upper bound. To make this readable, we pass merely functions
   * fetching the lower or upper bound from bounds and let
   * fetchAppropriateBounds take care of subindexing the indices stored in b
   * appropriately for each passed function.
   *
   * m::l is lower
   * m::u is upper
   *
   * These functions had to be duplicated from the original lower and upper
   * functions and fully qualified since we cannot pass templated functions as
   * an overload set.
   */
  return std::max({
    Temple::invoke(CMUpper, m::fetchAppropriateBounds(bounds, b, m::l, m::u, m::u, m::u, m::u)),
    Temple::invoke(CMUpper, m::fetchAppropriateBounds(bounds, b, m::u, m::l, m::l, m::u, m::u)),
    Temple::invoke(CMUpper, m::fetchAppropriateBounds(bounds, b, m::u, m::u, m::u, m::l, m::l))
  });
}

double lowerTetrangleLimit(
  const Eigen::MatrixXd& bounds,
  const std::array<unsigned, 4>& b
) {
  // See upperTetrangleLimit to explain the matrix below
  return std::min({
    Temple::invoke(CMLower, m::fetchAppropriateBounds(bounds, b, m::u, m::u, m::l, m::l, m::u)),
    Temple::invoke(CMLower, m::fetchAppropriateBounds(bounds, b, m::u, m::l, m::u, m::u, m::l)),
    Temple::invoke(CMLower, m::fetchAppropriateBounds(bounds, b, m::l, m::l, m::u, m::l, m::u)),
    Temple::invoke(CMLower, m::fetchAppropriateBounds(bounds, b, m::l, m::u, m::l, m::u, m::l))
  });
}

struct TetrangleLimits {
  DistanceGeometry::ValueBounds klLimits;
  bool boundViolation;

  TetrangleLimits(
    const Eigen::MatrixXd& bounds,
    const std::array<unsigned, 4>& b
  ) {
    LU matrixPair {bounds, b};
    // Can try the original triCheck here too
    TriCheckResult check = triCheck(matrixPair.L, matrixPair.U);

    /* NOTE: upperTetrangleLimit and lowerTetrangleLimit could use the LU
     * matrices for better data locality if this ever needs optimization
     */
    if(check.u_col) {
      klLimits.upper = check.klUpperBound;
    } else {
      const double limit = upperTetrangleLimit(bounds, b);
      klLimits.upper = std::sqrt(limit);
    }

    if(check.l_col) {
      klLimits.lower = check.klLowerBound;
    } else {
      const double limit = lowerTetrangleLimit(bounds, b);
      klLimits.lower = std::sqrt(limit);
    }

    boundViolation = klLimits.upper < klLimits.lower;
  }
};

unsigned tetrangleSmooth(Eigen::Ref<Eigen::MatrixXd> bounds) {
  feenableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);

  const unsigned N = bounds.cols();

  // Minimal change in the bounds required to consider something has changed
  constexpr double epsilon = 0.01;

  bool changedSomething;
  unsigned iterations = 0;
  do {
    changedSomething = false;

    for(unsigned i = 0; i < N - 1; ++i) {
      for(unsigned j = i + 1; j < N; ++j) {
        // NOTE (i,j) and (k,l) are not mutually disjoint
        for(unsigned k = 0; k < N - 1; ++k) {
          for(unsigned l = k + 1; l < N; ++l) {
            // Equal index pairs are trouble
            if(i == k && j == l) {
              continue;
            }

            const TetrangleLimits limits {bounds, {i, j, k, l}};

            if(limits.boundViolation) {
              throw std::runtime_error("Bound violation found!");
            }

            // k < l, so bounds(k, l) is the upper bound, bounds(l, k) the lower
            double& klLowerBound = bounds(l, k);
            double& klUpperBound = bounds(k, l);

            assert(klLowerBound <= klUpperBound);

            if(
              limits.klLimits.lower > klLowerBound
              && std::fabs(limits.klLimits.lower - klLowerBound) / klLowerBound > epsilon
            ) {
              if(limits.klLimits.lower > klUpperBound) {
                throw std::runtime_error("Bound violation found!");
              }

              klLowerBound = limits.klLimits.lower;
              changedSomething = true;
            }

            if(
              limits.klLimits.upper < klUpperBound
              && std::fabs(klUpperBound - limits.klLimits.upper) / klUpperBound > epsilon
            ) {
              if(limits.klLimits.upper < klLowerBound) {
                throw std::runtime_error("Bound violation found!");
              }
              klUpperBound = limits.klLimits.upper;
              changedSomething = true;
            }
          }
        }
      }
    }

    ++iterations;
  } while(changedSomething);

  fedisableexcept(FE_DIVBYZERO | FE_INVALID | FE_OVERFLOW);
  return iterations;
}

} // namespace DistanceGeometry
} // namespace Molassembler
} // namespace Scine


/*
 *            Copyright 2009-2017 The VOTCA Development Team
 *                       (http://www.votca.org)
 *
 *      Licensed under the Apache License, Version 2.0 (the "License")
 *
 * You may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *              http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "A_ol I_ol" BA_olI_ol,
 * WITHOUT WARRANTIE_ol OR CONDITION_ol OF ANY KIND, either express or implied.
 * _olee the License for the specific language governing permissions and
 * limitations under the License.
 *
 */
// Overload of uBLAS prod function with MKL/GSL implementations

#include <votca/xtp/aomatrix.h>

#include <votca/xtp/aobasis.h>

#include <votca/tools/linalg.h>
#include <votca/xtp/elements.h>
#include <votca/tools/constants.h>
//#include <boost/timer/timer.hpp>

using namespace votca::tools;

namespace votca { namespace xtp {
    namespace ub = boost::numeric::ublas;
    namespace CTP = votca::ctp;

    void AODipole_Potential::FillBlock( ub::matrix_range< ub::matrix<double> >& _matrix,const AOShell* _shell_row,const AOShell* _shell_col , AOBasis* ecp) {

        const double pi = boost::math::constants::pi<double>();

        // Get components of dipole vector somehow

        vec dipole=-(apolarsite->getU1()+apolarsite->getQ1())*tools::conv::nm2bohr;

        double d_0 = dipole.getX();
        double d_1 = dipole.getY();
        double d_2 = dipole.getZ();

        // cout << _gridpoint << endl;
        // shell info, only lmax tells how far to go
        int _lmax_row = _shell_row->getLmax();
        int _lmax_col = _shell_col->getLmax();
        int _lsum = _lmax_row + _lmax_col;
        // set size of internal block for recursion
        int _nrows = this->getBlockSize( _lmax_row );
        int _ncols = this->getBlockSize( _lmax_col );

        // initialize local matrix block for unnormalized cartesians
/////////        ub::matrix<double> nuc   = ub::zero_matrix<double>(_nrows,_ncols);
        ub::matrix<double> dip = ub::zero_matrix<double>(_nrows,_ncols);

        //cout << nuc.size1() << ":" << nuc.size2() << endl;

        /* FOR CONTRACTED FUNCTIONS, ADD LOOP OVER ALL DECAYS IN CONTRACTION
         * MULTIPLY THE TRANSFORMATION MATRICES BY APPROPRIATE CONTRACTION
         * COEFFICIENTS, AND ADD TO matrix(i,j)
         */

 int n_orbitals[] = { 1, 4, 10, 20, 35, 56, 84 };

 int nx[] = { 0,
              1, 0, 0,
              2, 1, 1, 0, 0, 0,
              3, 2, 2, 1, 1, 1, 0, 0, 0, 0,
              4, 3, 3, 2, 2, 2, 1, 1, 1, 1, 0, 0, 0, 0, 0 };

 int ny[] = { 0,
              0, 1, 0,
              0, 1, 0, 2, 1, 0,
              0, 1, 0, 2, 1, 0, 3, 2, 1, 0,
              0, 1, 0, 2, 1, 0, 3, 2, 1, 0, 4, 3, 2, 1, 0 };

 int nz[] = { 0,
              0, 0, 1,
              0, 0, 1, 0, 1, 2,
              0, 0, 1, 0, 1, 2, 0, 1, 2, 3,
              0, 0, 1, 0, 1, 2, 0, 1, 2, 3, 0, 1, 2, 3, 4 };

 int i_less_x[] = {  0,
                     0,  0,  0,
                     1,  2,  3,  0,  0,  0,
                     4,  5,  6,  7,  8,  9,  0,  0,  0,  0,
                    10, 11, 12, 13, 14, 15, 16, 17, 18, 19,  0,  0,  0,  0,  0 };

 int i_less_y[] = {  0,
                     0,  0,  0,
                     0,  1,  0,  2,  3,  0,
                     0,  4,  0,  5,  6,  0,  7,  8,  9,  0,
                     0, 10,  0, 11, 12,  0, 13, 14, 15,  0, 16, 17, 18, 19,  0 };

 int i_less_z[] = {  0,
                     0,  0,  0,
                     0,  0,  1,  0,  2,  3,
                     0,  0,  4,  0,  5,  6,  0,  7,  8,  9,
                     0,  0, 10,  0, 11, 12,  0, 13, 14, 15,  0, 16, 17, 18, 19 };

        // get shell positions
        const vec& _pos_row = _shell_row->getPos();
        const vec& _pos_col = _shell_col->getPos();
        const vec  _diff    = _pos_row - _pos_col;
        // initialize some helper

        double _distsq = _diff*_diff;

        // iterate over Gaussians in this _shell_row
        for (AOShell::GaussianIterator itr = _shell_row->firstGaussian(); itr != _shell_row->lastGaussian(); ++itr) {
            // iterate over Gaussians in this _shell_col
            // get decay constant
            const double _decay_row = itr->getDecay();

            for ( AOShell::GaussianIterator itc = _shell_col->firstGaussian(); itc != _shell_col->lastGaussian(); ++itc) {
                //get decay constant
                const double _decay_col = itc->getDecay();

                const double zeta = _decay_row + _decay_col;
                const double _fak  = 0.5/zeta;
                const double _fak2 = 2.0 * _fak;
                const double xi = _decay_row * _decay_col * _fak2;

                double _exparg = xi *_distsq;
                // check if distance between postions is big, then skip step
                if ( _exparg > 30.0 ) { continue; }

        // some helpers
        double PmA0 = _fak2*( _decay_row * _pos_row.getX() + _decay_col * _pos_col.getX() ) - _pos_row.getX();
        double PmA1 = _fak2*( _decay_row * _pos_row.getY() + _decay_col * _pos_col.getY() ) - _pos_row.getY();
        double PmA2 = _fak2*( _decay_row * _pos_row.getZ() + _decay_col * _pos_col.getZ() ) - _pos_row.getZ();

        double PmB0 = _fak2*( _decay_row * _pos_row.getX() + _decay_col * _pos_col.getX() ) - _pos_col.getX();
        double PmB1 = _fak2*( _decay_row * _pos_row.getY() + _decay_col * _pos_col.getY() ) - _pos_col.getY();
        double PmB2 = _fak2*( _decay_row * _pos_row.getZ() + _decay_col * _pos_col.getZ() ) - _pos_col.getZ();

        double PmC0 = _fak2*( _decay_row * _pos_row.getX() + _decay_col * _pos_col.getX() ) - _gridpoint.getX();
        double PmC1 = _fak2*( _decay_row * _pos_row.getY() + _decay_col * _pos_col.getY() ) - _gridpoint.getY();
        double PmC2 = _fak2*( _decay_row * _pos_row.getZ() + _decay_col * _pos_col.getZ() ) - _gridpoint.getZ();

        const double _U = zeta*(PmC0*PmC0+PmC1*PmC1+PmC2*PmC2);

        const std::vector<double> _FmU=XIntegrate(_lsum+2, _U );

        typedef boost::multi_array<double, 3> ma_type;
        typedef boost::multi_array<double, 4> ma4_type; //////////////////
        ma_type nuc3(boost::extents[_nrows][_ncols][_lsum+1]);
        ma4_type dip4(boost::extents[_nrows][_ncols][3][_lsum+1]);
        typedef ma_type::index index;

        for (index i = 0; i < _nrows; ++i) {
          for (index j = 0; j < _ncols; ++j) {
            for (index m = 0; m < _lsum+1; ++m) {
              nuc3[i][j][m] = 0.;
            }
          }
        }

        for (index i = 0; i < _nrows; ++i) {
          for (index j = 0; j < _ncols; ++j) {
            for (index k = 0; k < 3 ; ++k) { ///////////////////////////////// error corrected
              for (index m = 0; m < _lsum+1; ++m) {
                dip4[i][j][k][m] = 0.;
              }
            }
          }
        }

// (s-s element normiert )
double _prefactor = 4. * sqrt(2./pi) * pow(_decay_row*_decay_col,.75) * _fak2 * exp(-_exparg);
for (int m = 0; m < _lsum+1; m++) {
  nuc3[0][0][m] = _prefactor*_FmU[m];
}
//------------------------------------------------------

//Integrals     p - s
if (_lmax_row > 0) {
  for (int m = 0; m < _lsum; m++) {
    nuc3[Cart::x][0][m] = PmA0*nuc3[0][0][m] - PmC0*nuc3[0][0][m+1];
    nuc3[Cart::y][0][m] = PmA1*nuc3[0][0][m] - PmC1*nuc3[0][0][m+1];
    nuc3[Cart::z][0][m] = PmA2*nuc3[0][0][m] - PmC2*nuc3[0][0][m+1];
  }
}
//------------------------------------------------------

//Integrals     d - s
if (_lmax_row > 1) {
  for (int m = 0; m < _lsum-1; m++) {
    double term = _fak*(nuc3[0][0][m]-nuc3[0][0][m+1]);
    nuc3[Cart::xx][0][m] = PmA0*nuc3[Cart::x][0][m] - PmC0*nuc3[Cart::x][0][m+1] + term;
    nuc3[Cart::xy][0][m] = PmA0*nuc3[Cart::y][0][m] - PmC0*nuc3[Cart::y][0][m+1];
    nuc3[Cart::xz][0][m] = PmA0*nuc3[Cart::z][0][m] - PmC0*nuc3[Cart::z][0][m+1];
    nuc3[Cart::yy][0][m] = PmA1*nuc3[Cart::y][0][m] - PmC1*nuc3[Cart::y][0][m+1] + term;
    nuc3[Cart::yz][0][m] = PmA1*nuc3[Cart::z][0][m] - PmC1*nuc3[Cart::z][0][m+1];
    nuc3[Cart::zz][0][m] = PmA2*nuc3[Cart::z][0][m] - PmC2*nuc3[Cart::z][0][m+1] + term;
  }
}
//------------------------------------------------------

//Integrals     f - s
if (_lmax_row > 2) {
  for (int m = 0; m < _lsum-2; m++) {
    nuc3[Cart::xxx][0][m] = PmA0*nuc3[Cart::xx][0][m] - PmC0*nuc3[Cart::xx][0][m+1] + 2*_fak*(nuc3[Cart::x][0][m]-nuc3[Cart::x][0][m+1]);
    nuc3[Cart::xxy][0][m] = PmA1*nuc3[Cart::xx][0][m] - PmC1*nuc3[Cart::xx][0][m+1];
    nuc3[Cart::xxz][0][m] = PmA2*nuc3[Cart::xx][0][m] - PmC2*nuc3[Cart::xx][0][m+1];
    nuc3[Cart::xyy][0][m] = PmA0*nuc3[Cart::yy][0][m] - PmC0*nuc3[Cart::yy][0][m+1];
    nuc3[Cart::xyz][0][m] = PmA0*nuc3[Cart::yz][0][m] - PmC0*nuc3[Cart::yz][0][m+1];
    nuc3[Cart::xzz][0][m] = PmA0*nuc3[Cart::zz][0][m] - PmC0*nuc3[Cart::zz][0][m+1];
    nuc3[Cart::yyy][0][m] = PmA1*nuc3[Cart::yy][0][m] - PmC1*nuc3[Cart::yy][0][m+1] + 2*_fak*(nuc3[Cart::y][0][m]-nuc3[Cart::y][0][m+1]);
    nuc3[Cart::yyz][0][m] = PmA2*nuc3[Cart::yy][0][m] - PmC2*nuc3[Cart::yy][0][m+1];
    nuc3[Cart::yzz][0][m] = PmA1*nuc3[Cart::zz][0][m] - PmC1*nuc3[Cart::zz][0][m+1];
    nuc3[Cart::zzz][0][m] = PmA2*nuc3[Cart::zz][0][m] - PmC2*nuc3[Cart::zz][0][m+1] + 2*_fak*(nuc3[Cart::z][0][m]-nuc3[Cart::z][0][m+1]);
  }
}
//------------------------------------------------------

//Integrals     g - s
if (_lmax_row > 3) {
  for (int m = 0; m < _lsum-3; m++) {
    double term_xx = _fak*(nuc3[Cart::xx][0][m]-nuc3[Cart::xx][0][m+1]);
    double term_yy = _fak*(nuc3[Cart::yy][0][m]-nuc3[Cart::yy][0][m+1]);
    double term_zz = _fak*(nuc3[Cart::zz][0][m]-nuc3[Cart::zz][0][m+1]);
    nuc3[Cart::xxxx][0][m] = PmA0*nuc3[Cart::xxx][0][m] - PmC0*nuc3[Cart::xxx][0][m+1] + 3*term_xx;
    nuc3[Cart::xxxy][0][m] = PmA1*nuc3[Cart::xxx][0][m] - PmC1*nuc3[Cart::xxx][0][m+1];
    nuc3[Cart::xxxz][0][m] = PmA2*nuc3[Cart::xxx][0][m] - PmC2*nuc3[Cart::xxx][0][m+1];
    nuc3[Cart::xxyy][0][m] = PmA0*nuc3[Cart::xyy][0][m] - PmC0*nuc3[Cart::xyy][0][m+1] + term_yy;
    nuc3[Cart::xxyz][0][m] = PmA1*nuc3[Cart::xxz][0][m] - PmC1*nuc3[Cart::xxz][0][m+1];
    nuc3[Cart::xxzz][0][m] = PmA0*nuc3[Cart::xzz][0][m] - PmC0*nuc3[Cart::xzz][0][m+1] + term_zz;
    nuc3[Cart::xyyy][0][m] = PmA0*nuc3[Cart::yyy][0][m] - PmC0*nuc3[Cart::yyy][0][m+1];
    nuc3[Cart::xyyz][0][m] = PmA0*nuc3[Cart::yyz][0][m] - PmC0*nuc3[Cart::yyz][0][m+1];
    nuc3[Cart::xyzz][0][m] = PmA0*nuc3[Cart::yzz][0][m] - PmC0*nuc3[Cart::yzz][0][m+1];
    nuc3[Cart::xzzz][0][m] = PmA0*nuc3[Cart::zzz][0][m] - PmC0*nuc3[Cart::zzz][0][m+1];
    nuc3[Cart::yyyy][0][m] = PmA1*nuc3[Cart::yyy][0][m] - PmC1*nuc3[Cart::yyy][0][m+1] + 3*term_yy;
    nuc3[Cart::yyyz][0][m] = PmA2*nuc3[Cart::yyy][0][m] - PmC2*nuc3[Cart::yyy][0][m+1];
    nuc3[Cart::yyzz][0][m] = PmA1*nuc3[Cart::yzz][0][m] - PmC1*nuc3[Cart::yzz][0][m+1] + term_zz;
    nuc3[Cart::yzzz][0][m] = PmA1*nuc3[Cart::zzz][0][m] - PmC1*nuc3[Cart::zzz][0][m+1];
    nuc3[Cart::zzzz][0][m] = PmA2*nuc3[Cart::zzz][0][m] - PmC2*nuc3[Cart::zzz][0][m+1] + 3*term_zz;
  }
}
//------------------------------------------------------

if (_lmax_col > 0) {

  //Integrals     s - p
  for (int m = 0; m < _lmax_col; m++) {
    nuc3[0][Cart::x][m] = PmB0*nuc3[0][0][m] - PmC0*nuc3[0][0][m+1];
    nuc3[0][Cart::y][m] = PmB1*nuc3[0][0][m] - PmC1*nuc3[0][0][m+1];
    nuc3[0][Cart::z][m] = PmB2*nuc3[0][0][m] - PmC2*nuc3[0][0][m+1];
  }
  //------------------------------------------------------

  //Integrals     p - p
  if (_lmax_row > 0) {
    for (int m = 0; m < _lmax_col; m++) {
      double term = _fak*(nuc3[0][0][m]-nuc3[0][0][m+1]);
      for (int _i =  1; _i < 4; _i++) {
        nuc3[_i][Cart::x][m] = PmB0*nuc3[_i][0][m] - PmC0*nuc3[_i][0][m+1] + nx[_i]*term;
        nuc3[_i][Cart::y][m] = PmB1*nuc3[_i][0][m] - PmC1*nuc3[_i][0][m+1] + ny[_i]*term;
        nuc3[_i][Cart::z][m] = PmB2*nuc3[_i][0][m] - PmC2*nuc3[_i][0][m+1] + nz[_i]*term;
      }
    }
  }
  //------------------------------------------------------

  //Integrals     d - p     f - p     g - p
  for (int m = 0; m < _lmax_col; m++) {
    for (int _i = 4; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      nuc3[_i][Cart::x][m] = PmB0*nuc3[_i][0][m] - PmC0*nuc3[_i][0][m+1] + nx_i*_fak*(nuc3[ilx_i][0][m] - nuc3[ilx_i][0][m+1]);
      nuc3[_i][Cart::y][m] = PmB1*nuc3[_i][0][m] - PmC1*nuc3[_i][0][m+1] + ny_i*_fak*(nuc3[ily_i][0][m] - nuc3[ily_i][0][m+1]);
      nuc3[_i][Cart::z][m] = PmB2*nuc3[_i][0][m] - PmC2*nuc3[_i][0][m+1] + nz_i*_fak*(nuc3[ilz_i][0][m] - nuc3[ilz_i][0][m+1]);
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 0)

if (_lmax_col > 1) {

  //Integrals     s - d
  for (int m = 0; m < _lmax_col-1; m++) {
    double term = _fak*(nuc3[0][0][m]-nuc3[0][0][m+1]);
    nuc3[0][Cart::xx][m] = PmB0*nuc3[0][Cart::x][m] - PmC0*nuc3[0][Cart::x][m+1] + term;
    nuc3[0][Cart::xy][m] = PmB0*nuc3[0][Cart::y][m] - PmC0*nuc3[0][Cart::y][m+1];
    nuc3[0][Cart::xz][m] = PmB0*nuc3[0][Cart::z][m] - PmC0*nuc3[0][Cart::z][m+1];
    nuc3[0][Cart::yy][m] = PmB1*nuc3[0][Cart::y][m] - PmC1*nuc3[0][Cart::y][m+1] + term;
    nuc3[0][Cart::yz][m] = PmB1*nuc3[0][Cart::z][m] - PmC1*nuc3[0][Cart::z][m+1];
    nuc3[0][Cart::zz][m] = PmB2*nuc3[0][Cart::z][m] - PmC2*nuc3[0][Cart::z][m+1] + term;
  }
  //------------------------------------------------------

  //Integrals     p - d     d - d     f - d     g - d
  for (int m = 0; m < _lmax_col-1; m++) {
    for (int _i = 1; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      double term = _fak*(nuc3[_i][0][m]-nuc3[_i][0][m+1]);
      nuc3[_i][Cart::xx][m] = PmB0*nuc3[_i][Cart::x][m] - PmC0*nuc3[_i][Cart::x][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::x][m] - nuc3[ilx_i][Cart::x][m+1]) + term;
      nuc3[_i][Cart::xy][m] = PmB0*nuc3[_i][Cart::y][m] - PmC0*nuc3[_i][Cart::y][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::y][m] - nuc3[ilx_i][Cart::y][m+1]);
      nuc3[_i][Cart::xz][m] = PmB0*nuc3[_i][Cart::z][m] - PmC0*nuc3[_i][Cart::z][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::z][m] - nuc3[ilx_i][Cart::z][m+1]);
      nuc3[_i][Cart::yy][m] = PmB1*nuc3[_i][Cart::y][m] - PmC1*nuc3[_i][Cart::y][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::y][m] - nuc3[ily_i][Cart::y][m+1]) + term;
      nuc3[_i][Cart::yz][m] = PmB1*nuc3[_i][Cart::z][m] - PmC1*nuc3[_i][Cart::z][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::z][m] - nuc3[ily_i][Cart::z][m+1]);
      nuc3[_i][Cart::zz][m] = PmB2*nuc3[_i][Cart::z][m] - PmC2*nuc3[_i][Cart::z][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::z][m] - nuc3[ilz_i][Cart::z][m+1]) + term;
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 1)

if (_lmax_col > 2) {

  //Integrals     s - f
  for (int m = 0; m < _lmax_col-2; m++) {
    nuc3[0][Cart::xxx][m] = PmB0*nuc3[0][Cart::xx][m] - PmC0*nuc3[0][Cart::xx][m+1] + 2*_fak*(nuc3[0][Cart::x][m]-nuc3[0][Cart::x][m+1]);
    nuc3[0][Cart::xxy][m] = PmB1*nuc3[0][Cart::xx][m] - PmC1*nuc3[0][Cart::xx][m+1];
    nuc3[0][Cart::xxz][m] = PmB2*nuc3[0][Cart::xx][m] - PmC2*nuc3[0][Cart::xx][m+1];
    nuc3[0][Cart::xyy][m] = PmB0*nuc3[0][Cart::yy][m] - PmC0*nuc3[0][Cart::yy][m+1];
    nuc3[0][Cart::xyz][m] = PmB0*nuc3[0][Cart::yz][m] - PmC0*nuc3[0][Cart::yz][m+1];
    nuc3[0][Cart::xzz][m] = PmB0*nuc3[0][Cart::zz][m] - PmC0*nuc3[0][Cart::zz][m+1];
    nuc3[0][Cart::yyy][m] = PmB1*nuc3[0][Cart::yy][m] - PmC1*nuc3[0][Cart::yy][m+1] + 2*_fak*(nuc3[0][Cart::y][m]-nuc3[0][Cart::y][m+1]);
    nuc3[0][Cart::yyz][m] = PmB2*nuc3[0][Cart::yy][m] - PmC2*nuc3[0][Cart::yy][m+1];
    nuc3[0][Cart::yzz][m] = PmB1*nuc3[0][Cart::zz][m] - PmC1*nuc3[0][Cart::zz][m+1];
    nuc3[0][Cart::zzz][m] = PmB2*nuc3[0][Cart::zz][m] - PmC2*nuc3[0][Cart::zz][m+1] + 2*_fak*(nuc3[0][Cart::z][m]-nuc3[0][Cart::z][m+1]);
  }
  //------------------------------------------------------

  //Integrals     p - f     d - f     f - f     g - f
  for (int m = 0; m < _lmax_col-2; m++) {
    for (int _i = 1; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      double term_x = 2*_fak*(nuc3[_i][Cart::x][m]-nuc3[_i][Cart::x][m+1]);
      double term_y = 2*_fak*(nuc3[_i][Cart::y][m]-nuc3[_i][Cart::y][m+1]);
      double term_z = 2*_fak*(nuc3[_i][Cart::z][m]-nuc3[_i][Cart::z][m+1]);
      nuc3[_i][Cart::xxx][m] = PmB0*nuc3[_i][Cart::xx][m] - PmC0*nuc3[_i][Cart::xx][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::xx][m] - nuc3[ilx_i][Cart::xx][m+1]) + term_x;
      nuc3[_i][Cart::xxy][m] = PmB1*nuc3[_i][Cart::xx][m] - PmC1*nuc3[_i][Cart::xx][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::xx][m] - nuc3[ily_i][Cart::xx][m+1]);
      nuc3[_i][Cart::xxz][m] = PmB2*nuc3[_i][Cart::xx][m] - PmC2*nuc3[_i][Cart::xx][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::xx][m] - nuc3[ilz_i][Cart::xx][m+1]);
      nuc3[_i][Cart::xyy][m] = PmB0*nuc3[_i][Cart::yy][m] - PmC0*nuc3[_i][Cart::yy][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::yy][m] - nuc3[ilx_i][Cart::yy][m+1]);
      nuc3[_i][Cart::xyz][m] = PmB0*nuc3[_i][Cart::yz][m] - PmC0*nuc3[_i][Cart::yz][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::yz][m] - nuc3[ilx_i][Cart::yz][m+1]);
      nuc3[_i][Cart::xzz][m] = PmB0*nuc3[_i][Cart::zz][m] - PmC0*nuc3[_i][Cart::zz][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::zz][m] - nuc3[ilx_i][Cart::zz][m+1]);
      nuc3[_i][Cart::yyy][m] = PmB1*nuc3[_i][Cart::yy][m] - PmC1*nuc3[_i][Cart::yy][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::yy][m] - nuc3[ily_i][Cart::yy][m+1]) + term_y;
      nuc3[_i][Cart::yyz][m] = PmB2*nuc3[_i][Cart::yy][m] - PmC2*nuc3[_i][Cart::yy][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::yy][m] - nuc3[ilz_i][Cart::yy][m+1]);
      nuc3[_i][Cart::yzz][m] = PmB1*nuc3[_i][Cart::zz][m] - PmC1*nuc3[_i][Cart::zz][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::zz][m] - nuc3[ily_i][Cart::zz][m+1]);
      nuc3[_i][Cart::zzz][m] = PmB2*nuc3[_i][Cart::zz][m] - PmC2*nuc3[_i][Cart::zz][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::zz][m] - nuc3[ilz_i][Cart::zz][m+1]) + term_z;
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 2)

if (_lmax_col > 3) {

  //Integrals     s - g
  for (int m = 0; m < _lmax_col-3; m++) {
    double term_xx = _fak*(nuc3[0][Cart::xx][m]-nuc3[0][Cart::xx][m+1]);
    double term_yy = _fak*(nuc3[0][Cart::yy][m]-nuc3[0][Cart::yy][m+1]);
    double term_zz = _fak*(nuc3[0][Cart::zz][m]-nuc3[0][Cart::zz][m+1]);
    nuc3[0][Cart::xxxx][m] = PmB0*nuc3[0][Cart::xxx][m] - PmC0*nuc3[0][Cart::xxx][m+1] + 3*term_xx;
    nuc3[0][Cart::xxxy][m] = PmB1*nuc3[0][Cart::xxx][m] - PmC1*nuc3[0][Cart::xxx][m+1];
    nuc3[0][Cart::xxxz][m] = PmB2*nuc3[0][Cart::xxx][m] - PmC2*nuc3[0][Cart::xxx][m+1];
    nuc3[0][Cart::xxyy][m] = PmB0*nuc3[0][Cart::xyy][m] - PmC0*nuc3[0][Cart::xyy][m+1] + term_yy;
    nuc3[0][Cart::xxyz][m] = PmB1*nuc3[0][Cart::xxz][m] - PmC1*nuc3[0][Cart::xxz][m+1];
    nuc3[0][Cart::xxzz][m] = PmB0*nuc3[0][Cart::xzz][m] - PmC0*nuc3[0][Cart::xzz][m+1] + term_zz;
    nuc3[0][Cart::xyyy][m] = PmB0*nuc3[0][Cart::yyy][m] - PmC0*nuc3[0][Cart::yyy][m+1];
    nuc3[0][Cart::xyyz][m] = PmB0*nuc3[0][Cart::yyz][m] - PmC0*nuc3[0][Cart::yyz][m+1];
    nuc3[0][Cart::xyzz][m] = PmB0*nuc3[0][Cart::yzz][m] - PmC0*nuc3[0][Cart::yzz][m+1];
    nuc3[0][Cart::xzzz][m] = PmB0*nuc3[0][Cart::zzz][m] - PmC0*nuc3[0][Cart::zzz][m+1];
    nuc3[0][Cart::yyyy][m] = PmB1*nuc3[0][Cart::yyy][m] - PmC1*nuc3[0][Cart::yyy][m+1] + 3*term_yy;
    nuc3[0][Cart::yyyz][m] = PmB2*nuc3[0][Cart::yyy][m] - PmC2*nuc3[0][Cart::yyy][m+1];
    nuc3[0][Cart::yyzz][m] = PmB1*nuc3[0][Cart::yzz][m] - PmC1*nuc3[0][Cart::yzz][m+1] + term_zz;
    nuc3[0][Cart::yzzz][m] = PmB1*nuc3[0][Cart::zzz][m] - PmC1*nuc3[0][Cart::zzz][m+1];
    nuc3[0][Cart::zzzz][m] = PmB2*nuc3[0][Cart::zzz][m] - PmC2*nuc3[0][Cart::zzz][m+1] + 3*term_zz;
  }
  //------------------------------------------------------

  //Integrals     p - g     d - g     f - g     g - g
  for (int m = 0; m < _lmax_col-3; m++) {
    for (int _i = 1; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      double term_xx = _fak*(nuc3[_i][Cart::xx][m]-nuc3[_i][Cart::xx][m+1]);
      double term_yy = _fak*(nuc3[_i][Cart::yy][m]-nuc3[_i][Cart::yy][m+1]);
      double term_zz = _fak*(nuc3[_i][Cart::zz][m]-nuc3[_i][Cart::zz][m+1]);
      nuc3[_i][Cart::xxxx][m] = PmB0*nuc3[_i][Cart::xxx][m] - PmC0*nuc3[_i][Cart::xxx][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::xxx][m] - nuc3[ilx_i][Cart::xxx][m+1]) + 3*term_xx;
      nuc3[_i][Cart::xxxy][m] = PmB1*nuc3[_i][Cart::xxx][m] - PmC1*nuc3[_i][Cart::xxx][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::xxx][m] - nuc3[ily_i][Cart::xxx][m+1]);
      nuc3[_i][Cart::xxxz][m] = PmB2*nuc3[_i][Cart::xxx][m] - PmC2*nuc3[_i][Cart::xxx][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::xxx][m] - nuc3[ilz_i][Cart::xxx][m+1]);
      nuc3[_i][Cart::xxyy][m] = PmB0*nuc3[_i][Cart::xyy][m] - PmC0*nuc3[_i][Cart::xyy][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::xyy][m] - nuc3[ilx_i][Cart::xyy][m+1]) + term_yy;
      nuc3[_i][Cart::xxyz][m] = PmB1*nuc3[_i][Cart::xxz][m] - PmC1*nuc3[_i][Cart::xxz][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::xxz][m] - nuc3[ily_i][Cart::xxz][m+1]);
      nuc3[_i][Cart::xxzz][m] = PmB0*nuc3[_i][Cart::xzz][m] - PmC0*nuc3[_i][Cart::xzz][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::xzz][m] - nuc3[ilx_i][Cart::xzz][m+1]) + term_zz;
      nuc3[_i][Cart::xyyy][m] = PmB0*nuc3[_i][Cart::yyy][m] - PmC0*nuc3[_i][Cart::yyy][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::yyy][m] - nuc3[ilx_i][Cart::yyy][m+1]);
      nuc3[_i][Cart::xyyz][m] = PmB0*nuc3[_i][Cart::yyz][m] - PmC0*nuc3[_i][Cart::yyz][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::yyz][m] - nuc3[ilx_i][Cart::yyz][m+1]);
      nuc3[_i][Cart::xyzz][m] = PmB0*nuc3[_i][Cart::yzz][m] - PmC0*nuc3[_i][Cart::yzz][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::yzz][m] - nuc3[ilx_i][Cart::yzz][m+1]);
      nuc3[_i][Cart::xzzz][m] = PmB0*nuc3[_i][Cart::zzz][m] - PmC0*nuc3[_i][Cart::zzz][m+1] + nx_i*_fak*(nuc3[ilx_i][Cart::zzz][m] - nuc3[ilx_i][Cart::zzz][m+1]);
      nuc3[_i][Cart::yyyy][m] = PmB1*nuc3[_i][Cart::yyy][m] - PmC1*nuc3[_i][Cart::yyy][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::yyy][m] - nuc3[ily_i][Cart::yyy][m+1]) + 3*term_yy;
      nuc3[_i][Cart::yyyz][m] = PmB2*nuc3[_i][Cart::yyy][m] - PmC2*nuc3[_i][Cart::yyy][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::yyy][m] - nuc3[ilz_i][Cart::yyy][m+1]);
      nuc3[_i][Cart::yyzz][m] = PmB1*nuc3[_i][Cart::yzz][m] - PmC1*nuc3[_i][Cart::yzz][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::yzz][m] - nuc3[ily_i][Cart::yzz][m+1]) + term_zz;
      nuc3[_i][Cart::yzzz][m] = PmB1*nuc3[_i][Cart::zzz][m] - PmC1*nuc3[_i][Cart::zzz][m+1] + ny_i*_fak*(nuc3[ily_i][Cart::zzz][m] - nuc3[ily_i][Cart::zzz][m+1]);
      nuc3[_i][Cart::zzzz][m] = PmB2*nuc3[_i][Cart::zzz][m] - PmC2*nuc3[_i][Cart::zzz][m+1] + nz_i*_fak*(nuc3[ilz_i][Cart::zzz][m] - nuc3[ilz_i][Cart::zzz][m+1]) + 3*term_zz;
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 3)

// (s-s element normiert )
double _prefactor_dip = 2. * zeta * _prefactor;
for (int m = 0; m < _lsum+1; m++) {
  dip4[0][0][0][m] = PmC0*_prefactor_dip*_FmU[m+1];
  dip4[0][0][1][m] = PmC1*_prefactor_dip*_FmU[m+1];
  dip4[0][0][2][m] = PmC2*_prefactor_dip*_FmU[m+1];
}
//------------------------------------------------------

//Integrals     p - s
if (_lmax_row > 0) {
  for (int m = 0; m < _lsum; m++) {
    for (int _k = 0; _k < 3; _k++) {
      dip4[Cart::x][0][_k][m] = PmA0*dip4[0][0][_k][m] - PmC0*dip4[0][0][_k][m+1] + (_k==0)*nuc3[0][0][m+1];
      dip4[Cart::y][0][_k][m] = PmA1*dip4[0][0][_k][m] - PmC1*dip4[0][0][_k][m+1] + (_k==1)*nuc3[0][0][m+1];
      dip4[Cart::z][0][_k][m] = PmA2*dip4[0][0][_k][m] - PmC2*dip4[0][0][_k][m+1] + (_k==2)*nuc3[0][0][m+1];
    }
  }
}
//------------------------------------------------------

//Integrals     d - s
if (_lmax_row > 1) {
  for (int m = 0; m < _lsum-1; m++) {
    for (int _k = 0; _k < 3; _k++) {
    double term = _fak*(dip4[0][0][_k][m]-dip4[0][0][_k][m+1]);
      dip4[Cart::xx][0][_k][m] = PmA0*dip4[Cart::x][0][_k][m] - PmC0*dip4[Cart::x][0][_k][m+1] + (_k==0)*nuc3[Cart::x][0][m+1] + term;
      dip4[Cart::xy][0][_k][m] = PmA0*dip4[Cart::y][0][_k][m] - PmC0*dip4[Cart::y][0][_k][m+1] + (_k==0)*nuc3[Cart::y][0][m+1];
      dip4[Cart::xz][0][_k][m] = PmA0*dip4[Cart::z][0][_k][m] - PmC0*dip4[Cart::z][0][_k][m+1] + (_k==0)*nuc3[Cart::z][0][m+1];
      dip4[Cart::yy][0][_k][m] = PmA1*dip4[Cart::y][0][_k][m] - PmC1*dip4[Cart::y][0][_k][m+1] + (_k==1)*nuc3[Cart::y][0][m+1] + term;
      dip4[Cart::yz][0][_k][m] = PmA1*dip4[Cart::z][0][_k][m] - PmC1*dip4[Cart::z][0][_k][m+1] + (_k==1)*nuc3[Cart::z][0][m+1];
      dip4[Cart::zz][0][_k][m] = PmA2*dip4[Cart::z][0][_k][m] - PmC2*dip4[Cart::z][0][_k][m+1] + (_k==2)*nuc3[Cart::z][0][m+1] + term;
    }
  }
}
//------------------------------------------------------

//Integrals     f - s
if (_lmax_row > 2) {
  for (int m = 0; m < _lsum-2; m++) {
    for (int _k = 0; _k < 3; _k++) {
      dip4[Cart::xxx][0][_k][m] = PmA0*dip4[Cart::xx][0][_k][m] - PmC0*dip4[Cart::xx][0][_k][m+1] + (_k==0)*nuc3[Cart::xx][0][m+1] + 2*_fak*(dip4[Cart::x][0][_k][m]-dip4[Cart::x][0][_k][m+1]);
      dip4[Cart::xxy][0][_k][m] = PmA1*dip4[Cart::xx][0][_k][m] - PmC1*dip4[Cart::xx][0][_k][m+1] + (_k==1)*nuc3[Cart::xx][0][m+1];
      dip4[Cart::xxz][0][_k][m] = PmA2*dip4[Cart::xx][0][_k][m] - PmC2*dip4[Cart::xx][0][_k][m+1] + (_k==2)*nuc3[Cart::xx][0][m+1];
      dip4[Cart::xyy][0][_k][m] = PmA0*dip4[Cart::yy][0][_k][m] - PmC0*dip4[Cart::yy][0][_k][m+1] + (_k==0)*nuc3[Cart::yy][0][m+1];
      dip4[Cart::xyz][0][_k][m] = PmA0*dip4[Cart::yz][0][_k][m] - PmC0*dip4[Cart::yz][0][_k][m+1] + (_k==0)*nuc3[Cart::yz][0][m+1];
      dip4[Cart::xzz][0][_k][m] = PmA0*dip4[Cart::zz][0][_k][m] - PmC0*dip4[Cart::zz][0][_k][m+1] + (_k==0)*nuc3[Cart::zz][0][m+1];
      dip4[Cart::yyy][0][_k][m] = PmA1*dip4[Cart::yy][0][_k][m] - PmC1*dip4[Cart::yy][0][_k][m+1] + (_k==1)*nuc3[Cart::yy][0][m+1] + 2*_fak*(dip4[Cart::y][0][_k][m]-dip4[Cart::y][0][_k][m+1]);
      dip4[Cart::yyz][0][_k][m] = PmA2*dip4[Cart::yy][0][_k][m] - PmC2*dip4[Cart::yy][0][_k][m+1] + (_k==2)*nuc3[Cart::yy][0][m+1];
      dip4[Cart::yzz][0][_k][m] = PmA1*dip4[Cart::zz][0][_k][m] - PmC1*dip4[Cart::zz][0][_k][m+1] + (_k==1)*nuc3[Cart::zz][0][m+1];
      dip4[Cart::zzz][0][_k][m] = PmA2*dip4[Cart::zz][0][_k][m] - PmC2*dip4[Cart::zz][0][_k][m+1] + (_k==2)*nuc3[Cart::zz][0][m+1] + 2*_fak*(dip4[Cart::z][0][_k][m]-dip4[Cart::z][0][_k][m+1]);
    }
  }
}
//------------------------------------------------------

//Integrals     g - s
if (_lmax_row > 3) {
  for (int m = 0; m < _lsum-3; m++) {
    for (int _k = 0; _k < 3; _k++) {
    double term_xx = _fak*(dip4[Cart::xx][0][_k][m]-dip4[Cart::xx][0][_k][m+1]);
    double term_yy = _fak*(dip4[Cart::yy][0][_k][m]-dip4[Cart::yy][0][_k][m+1]);
    double term_zz = _fak*(dip4[Cart::zz][0][_k][m]-dip4[Cart::zz][0][_k][m+1]);
    dip4[Cart::xxxx][0][_k][m] = PmA0*dip4[Cart::xxx][0][_k][m] - PmC0*dip4[Cart::xxx][0][_k][m+1] + (_k==0)*nuc3[Cart::xxx][0][m+1] + 3*term_xx;
    dip4[Cart::xxxy][0][_k][m] = PmA1*dip4[Cart::xxx][0][_k][m] - PmC1*dip4[Cart::xxx][0][_k][m+1] + (_k==1)*nuc3[Cart::xxx][0][m+1];
    dip4[Cart::xxxz][0][_k][m] = PmA2*dip4[Cart::xxx][0][_k][m] - PmC2*dip4[Cart::xxx][0][_k][m+1] + (_k==2)*nuc3[Cart::xxx][0][m+1];
    dip4[Cart::xxyy][0][_k][m] = PmA0*dip4[Cart::xyy][0][_k][m] - PmC0*dip4[Cart::xyy][0][_k][m+1] + (_k==0)*nuc3[Cart::xyy][0][m+1] + term_yy;
    dip4[Cart::xxyz][0][_k][m] = PmA1*dip4[Cart::xxz][0][_k][m] - PmC1*dip4[Cart::xxz][0][_k][m+1] + (_k==1)*nuc3[Cart::xxz][0][m+1];
    dip4[Cart::xxzz][0][_k][m] = PmA0*dip4[Cart::xzz][0][_k][m] - PmC0*dip4[Cart::xzz][0][_k][m+1] + (_k==0)*nuc3[Cart::xzz][0][m+1] + term_zz;
    dip4[Cart::xyyy][0][_k][m] = PmA0*dip4[Cart::yyy][0][_k][m] - PmC0*dip4[Cart::yyy][0][_k][m+1] + (_k==0)*nuc3[Cart::yyy][0][m+1];
    dip4[Cart::xyyz][0][_k][m] = PmA0*dip4[Cart::yyz][0][_k][m] - PmC0*dip4[Cart::yyz][0][_k][m+1] + (_k==0)*nuc3[Cart::yyz][0][m+1];
    dip4[Cart::xyzz][0][_k][m] = PmA0*dip4[Cart::yzz][0][_k][m] - PmC0*dip4[Cart::yzz][0][_k][m+1] + (_k==0)*nuc3[Cart::yzz][0][m+1];
    dip4[Cart::xzzz][0][_k][m] = PmA0*dip4[Cart::zzz][0][_k][m] - PmC0*dip4[Cart::zzz][0][_k][m+1] + (_k==0)*nuc3[Cart::zzz][0][m+1];
    dip4[Cart::yyyy][0][_k][m] = PmA1*dip4[Cart::yyy][0][_k][m] - PmC1*dip4[Cart::yyy][0][_k][m+1] + (_k==1)*nuc3[Cart::yyy][0][m+1] + 3*term_yy;
    dip4[Cart::yyyz][0][_k][m] = PmA2*dip4[Cart::yyy][0][_k][m] - PmC2*dip4[Cart::yyy][0][_k][m+1] + (_k==2)*nuc3[Cart::yyy][0][m+1];
    dip4[Cart::yyzz][0][_k][m] = PmA1*dip4[Cart::yzz][0][_k][m] - PmC1*dip4[Cart::yzz][0][_k][m+1] + (_k==1)*nuc3[Cart::yzz][0][m+1] + term_zz;
    dip4[Cart::yzzz][0][_k][m] = PmA1*dip4[Cart::zzz][0][_k][m] - PmC1*dip4[Cart::zzz][0][_k][m+1] + (_k==1)*nuc3[Cart::zzz][0][m+1];
    dip4[Cart::zzzz][0][_k][m] = PmA2*dip4[Cart::zzz][0][_k][m] - PmC2*dip4[Cart::zzz][0][_k][m+1] + (_k==2)*nuc3[Cart::zzz][0][m+1] + 3*term_zz;
    }
  }
}
//------------------------------------------------------

if (_lmax_col > 0) {

  //Integrals     s - p
  for (int m = 0; m < _lmax_col; m++) {
    for (int _k = 0; _k < 3; _k++) {
      dip4[0][Cart::x][_k][m] = PmB0*dip4[0][0][_k][m] - PmC0*dip4[0][0][_k][m+1] + (_k==0)*nuc3[0][0][m+1];
      dip4[0][Cart::y][_k][m] = PmB1*dip4[0][0][_k][m] - PmC1*dip4[0][0][_k][m+1] + (_k==1)*nuc3[0][0][m+1];
      dip4[0][Cart::z][_k][m] = PmB2*dip4[0][0][_k][m] - PmC2*dip4[0][0][_k][m+1] + (_k==2)*nuc3[0][0][m+1];
    }
  }
  //------------------------------------------------------

  //Integrals     p - p
  if (_lmax_row > 0) {
    for (int m = 0; m < _lmax_col; m++) {
      for (int _i =  1; _i < 4; _i++) {
        for (int _k = 0; _k < 3; _k++) {
          double term = _fak*(dip4[0][0][_k][m]-dip4[0][0][_k][m+1]);
          dip4[_i][Cart::x][_k][m] = PmB0*dip4[_i][0][_k][m] - PmC0*dip4[_i][0][_k][m+1] + (_k==0)*nuc3[_i][0][m+1] + nx[_i]*term;
          dip4[_i][Cart::y][_k][m] = PmB1*dip4[_i][0][_k][m] - PmC1*dip4[_i][0][_k][m+1] + (_k==1)*nuc3[_i][0][m+1] + ny[_i]*term;
          dip4[_i][Cart::z][_k][m] = PmB2*dip4[_i][0][_k][m] - PmC2*dip4[_i][0][_k][m+1] + (_k==2)*nuc3[_i][0][m+1] + nz[_i]*term;
        }
      }
    }
  }
  //------------------------------------------------------

  //Integrals     d - p     f - p     g - p
  for (int m = 0; m < _lmax_col; m++) {
    for (int _i = 4; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      for (int _k = 0; _k < 3; _k++) {
        dip4[_i][Cart::x][_k][m] = PmB0*dip4[_i][0][_k][m] - PmC0*dip4[_i][0][_k][m+1] + (_k==0)*nuc3[_i][0][m+1] + nx_i*_fak*(dip4[ilx_i][0][_k][m] - dip4[ilx_i][0][_k][m+1]);
        dip4[_i][Cart::y][_k][m] = PmB1*dip4[_i][0][_k][m] - PmC1*dip4[_i][0][_k][m+1] + (_k==1)*nuc3[_i][0][m+1] + ny_i*_fak*(dip4[ily_i][0][_k][m] - dip4[ily_i][0][_k][m+1]);
        dip4[_i][Cart::z][_k][m] = PmB2*dip4[_i][0][_k][m] - PmC2*dip4[_i][0][_k][m+1] + (_k==2)*nuc3[_i][0][m+1] + nz_i*_fak*(dip4[ilz_i][0][_k][m] - dip4[ilz_i][0][_k][m+1]);
      }
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 0)

if (_lmax_col > 1) {

  //Integrals     s - d
  for (int m = 0; m < _lmax_col-1; m++) {
    for (int _k = 0; _k < 3; _k++) {
      double term = _fak*(dip4[0][0][_k][m]-dip4[0][0][_k][m+1]);
      dip4[0][Cart::xx][_k][m] = PmB0*dip4[0][Cart::x][_k][m] - PmC0*dip4[0][Cart::x][_k][m+1] + (_k==0)*nuc3[0][Cart::x][m+1] + term;
      dip4[0][Cart::xy][_k][m] = PmB0*dip4[0][Cart::y][_k][m] - PmC0*dip4[0][Cart::y][_k][m+1] + (_k==0)*nuc3[0][Cart::y][m+1];
      dip4[0][Cart::xz][_k][m] = PmB0*dip4[0][Cart::z][_k][m] - PmC0*dip4[0][Cart::z][_k][m+1] + (_k==0)*nuc3[0][Cart::z][m+1];
      dip4[0][Cart::yy][_k][m] = PmB1*dip4[0][Cart::y][_k][m] - PmC1*dip4[0][Cart::y][_k][m+1] + (_k==1)*nuc3[0][Cart::y][m+1] + term;
      dip4[0][Cart::yz][_k][m] = PmB1*dip4[0][Cart::z][_k][m] - PmC1*dip4[0][Cart::z][_k][m+1] + (_k==1)*nuc3[0][Cart::z][m+1];
      dip4[0][Cart::zz][_k][m] = PmB2*dip4[0][Cart::z][_k][m] - PmC2*dip4[0][Cart::z][_k][m+1] + (_k==2)*nuc3[0][Cart::z][m+1] + term;
    }
  }
  //------------------------------------------------------

  //Integrals     p - d     d - d     f - d     g - d
  for (int m = 0; m < _lmax_col-1; m++) {
    for (int _i = 1; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      for (int _k = 0; _k < 3; _k++) {
        double term = _fak*(dip4[_i][0][_k][m]-dip4[_i][0][_k][m+1]);
        dip4[_i][Cart::xx][_k][m] = PmB0*dip4[_i][Cart::x][_k][m] - PmC0*dip4[_i][Cart::x][_k][m+1] + (_k==0)*nuc3[_i][Cart::x][m+1]
                                    + nx_i*_fak*(dip4[ilx_i][Cart::x][_k][m] - dip4[ilx_i][Cart::x][_k][m+1]) + term;
        dip4[_i][Cart::xy][_k][m] = PmB0*dip4[_i][Cart::y][_k][m] - PmC0*dip4[_i][Cart::y][_k][m+1] + (_k==0)*nuc3[_i][Cart::y][m+1]
                                    + nx_i*_fak*(dip4[ilx_i][Cart::y][_k][m] - dip4[ilx_i][Cart::y][_k][m+1]);
        dip4[_i][Cart::xz][_k][m] = PmB0*dip4[_i][Cart::z][_k][m] - PmC0*dip4[_i][Cart::z][_k][m+1] + (_k==0)*nuc3[_i][Cart::z][m+1]
                                    + nx_i*_fak*(dip4[ilx_i][Cart::z][_k][m] - dip4[ilx_i][Cart::z][_k][m+1]);
        dip4[_i][Cart::yy][_k][m] = PmB1*dip4[_i][Cart::y][_k][m] - PmC1*dip4[_i][Cart::y][_k][m+1] + (_k==1)*nuc3[_i][Cart::y][m+1]
                                    + ny_i*_fak*(dip4[ily_i][Cart::y][_k][m] - dip4[ily_i][Cart::y][_k][m+1]) + term;
        dip4[_i][Cart::yz][_k][m] = PmB1*dip4[_i][Cart::z][_k][m] - PmC1*dip4[_i][Cart::z][_k][m+1] + (_k==1)*nuc3[_i][Cart::z][m+1]
                                    + ny_i*_fak*(dip4[ily_i][Cart::z][_k][m] - dip4[ily_i][Cart::z][_k][m+1]);
        dip4[_i][Cart::zz][_k][m] = PmB2*dip4[_i][Cart::z][_k][m] - PmC2*dip4[_i][Cart::z][_k][m+1] + (_k==2)*nuc3[_i][Cart::z][m+1]
                                    + nz_i*_fak*(dip4[ilz_i][Cart::z][_k][m] - dip4[ilz_i][Cart::z][_k][m+1]) + term;
      }
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 1)

if (_lmax_col > 2) {

  //Integrals     s - f
  for (int m = 0; m < _lmax_col-2; m++) {
    for (int _k = 0; _k < 3; _k++) {
      dip4[0][Cart::xxx][_k][m] = PmB0*dip4[0][Cart::xx][_k][m] - PmC0*dip4[0][Cart::xx][_k][m+1] + (_k==0)*nuc3[0][Cart::xx][m+1] + 2*_fak*(dip4[0][Cart::x][_k][m]-dip4[0][Cart::x][_k][m+1]);
      dip4[0][Cart::xxy][_k][m] = PmB1*dip4[0][Cart::xx][_k][m] - PmC1*dip4[0][Cart::xx][_k][m+1] + (_k==1)*nuc3[0][Cart::xx][m+1];
      dip4[0][Cart::xxz][_k][m] = PmB2*dip4[0][Cart::xx][_k][m] - PmC2*dip4[0][Cart::xx][_k][m+1] + (_k==2)*nuc3[0][Cart::xx][m+1];
      dip4[0][Cart::xyy][_k][m] = PmB0*dip4[0][Cart::yy][_k][m] - PmC0*dip4[0][Cart::yy][_k][m+1] + (_k==0)*nuc3[0][Cart::yy][m+1];
      dip4[0][Cart::xyz][_k][m] = PmB0*dip4[0][Cart::yz][_k][m] - PmC0*dip4[0][Cart::yz][_k][m+1] + (_k==0)*nuc3[0][Cart::yz][m+1];
      dip4[0][Cart::xzz][_k][m] = PmB0*dip4[0][Cart::zz][_k][m] - PmC0*dip4[0][Cart::zz][_k][m+1] + (_k==0)*nuc3[0][Cart::zz][m+1];
      dip4[0][Cart::yyy][_k][m] = PmB1*dip4[0][Cart::yy][_k][m] - PmC1*dip4[0][Cart::yy][_k][m+1] + (_k==1)*nuc3[0][Cart::yy][m+1] + 2*_fak*(dip4[0][Cart::y][_k][m]-dip4[0][Cart::y][_k][m+1]);
      dip4[0][Cart::yyz][_k][m] = PmB2*dip4[0][Cart::yy][_k][m] - PmC2*dip4[0][Cart::yy][_k][m+1] + (_k==2)*nuc3[0][Cart::yy][m+1];
      dip4[0][Cart::yzz][_k][m] = PmB1*dip4[0][Cart::zz][_k][m] - PmC1*dip4[0][Cart::zz][_k][m+1] + (_k==1)*nuc3[0][Cart::zz][m+1];
      dip4[0][Cart::zzz][_k][m] = PmB2*dip4[0][Cart::zz][_k][m] - PmC2*dip4[0][Cart::zz][_k][m+1] + (_k==2)*nuc3[0][Cart::zz][m+1] + 2*_fak*(dip4[0][Cart::z][_k][m]-dip4[0][Cart::z][_k][m+1]);
    }
  }
  //------------------------------------------------------

  //Integrals     p - f     d - f     f - f     g - f
  for (int m = 0; m < _lmax_col-2; m++) {
    for (int _i = 1; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      for (int _k = 0; _k < 3; _k++) {
        double term_x = 2*_fak*(dip4[_i][Cart::x][_k][m]-dip4[_i][Cart::x][_k][m+1]);
        double term_y = 2*_fak*(dip4[_i][Cart::y][_k][m]-dip4[_i][Cart::y][_k][m+1]);
        double term_z = 2*_fak*(dip4[_i][Cart::z][_k][m]-dip4[_i][Cart::z][_k][m+1]);
        dip4[_i][Cart::xxx][_k][m] = PmB0*dip4[_i][Cart::xx][_k][m] - PmC0*dip4[_i][Cart::xx][_k][m+1] + (_k==0)*nuc3[_i][Cart::xx][m+1]
                                     + nx_i*_fak*(dip4[ilx_i][Cart::xx][_k][m] - dip4[ilx_i][Cart::xx][_k][m+1]) + term_x;
        dip4[_i][Cart::xxy][_k][m] = PmB1*dip4[_i][Cart::xx][_k][m] - PmC1*dip4[_i][Cart::xx][_k][m+1] + (_k==1)*nuc3[_i][Cart::xx][m+1]
                                     + ny_i*_fak*(dip4[ily_i][Cart::xx][_k][m] - dip4[ily_i][Cart::xx][_k][m+1]);
        dip4[_i][Cart::xxz][_k][m] = PmB2*dip4[_i][Cart::xx][_k][m] - PmC2*dip4[_i][Cart::xx][_k][m+1] + (_k==2)*nuc3[_i][Cart::xx][m+1]
                                     + nz_i*_fak*(dip4[ilz_i][Cart::xx][_k][m] - dip4[ilz_i][Cart::xx][_k][m+1]);
        dip4[_i][Cart::xyy][_k][m] = PmB0*dip4[_i][Cart::yy][_k][m] - PmC0*dip4[_i][Cart::yy][_k][m+1] + (_k==0)*nuc3[_i][Cart::yy][m+1]
                                     + nx_i*_fak*(dip4[ilx_i][Cart::yy][_k][m] - dip4[ilx_i][Cart::yy][_k][m+1]);
        dip4[_i][Cart::xyz][_k][m] = PmB0*dip4[_i][Cart::yz][_k][m] - PmC0*dip4[_i][Cart::yz][_k][m+1] + (_k==0)*nuc3[_i][Cart::yz][m+1]
                                     + nx_i*_fak*(dip4[ilx_i][Cart::yz][_k][m] - dip4[ilx_i][Cart::yz][_k][m+1]);
        dip4[_i][Cart::xzz][_k][m] = PmB0*dip4[_i][Cart::zz][_k][m] - PmC0*dip4[_i][Cart::zz][_k][m+1] + (_k==0)*nuc3[_i][Cart::zz][m+1]
                                     + nx_i*_fak*(dip4[ilx_i][Cart::zz][_k][m] - dip4[ilx_i][Cart::zz][_k][m+1]);
        dip4[_i][Cart::yyy][_k][m] = PmB1*dip4[_i][Cart::yy][_k][m] - PmC1*dip4[_i][Cart::yy][_k][m+1] + (_k==1)*nuc3[_i][Cart::yy][m+1]
                                     + ny_i*_fak*(dip4[ily_i][Cart::yy][_k][m] - dip4[ily_i][Cart::yy][_k][m+1]) + term_y;
        dip4[_i][Cart::yyz][_k][m] = PmB2*dip4[_i][Cart::yy][_k][m] - PmC2*dip4[_i][Cart::yy][_k][m+1] + (_k==2)*nuc3[_i][Cart::yy][m+1]
                                     + nz_i*_fak*(dip4[ilz_i][Cart::yy][_k][m] - dip4[ilz_i][Cart::yy][_k][m+1]);
        dip4[_i][Cart::yzz][_k][m] = PmB1*dip4[_i][Cart::zz][_k][m] - PmC1*dip4[_i][Cart::zz][_k][m+1] + (_k==1)*nuc3[_i][Cart::zz][m+1]
                                     + ny_i*_fak*(dip4[ily_i][Cart::zz][_k][m] - dip4[ily_i][Cart::zz][_k][m+1]);
        dip4[_i][Cart::zzz][_k][m] = PmB2*dip4[_i][Cart::zz][_k][m] - PmC2*dip4[_i][Cart::zz][_k][m+1] + (_k==2)*nuc3[_i][Cart::zz][m+1]
                                     + nz_i*_fak*(dip4[ilz_i][Cart::zz][_k][m] - dip4[ilz_i][Cart::zz][_k][m+1]) + term_z;
      }
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 2)

if (_lmax_col > 3) {

  //Integrals     s - g
  for (int m = 0; m < _lmax_col-3; m++) {
    for (int _k = 0; _k < 3; _k++) {
      double term_xx = _fak*(dip4[0][Cart::xx][_k][m]-dip4[0][Cart::xx][_k][m+1]);
      double term_yy = _fak*(dip4[0][Cart::yy][_k][m]-dip4[0][Cart::yy][_k][m+1]);
      double term_zz = _fak*(dip4[0][Cart::zz][_k][m]-dip4[0][Cart::zz][_k][m+1]);
      dip4[0][Cart::xxxx][_k][m] = PmB0*dip4[0][Cart::xxx][_k][m] - PmC0*dip4[0][Cart::xxx][_k][m+1] + (_k==0)*nuc3[0][Cart::xxx][m+1] + 3*term_xx;
      dip4[0][Cart::xxxy][_k][m] = PmB1*dip4[0][Cart::xxx][_k][m] - PmC1*dip4[0][Cart::xxx][_k][m+1] + (_k==1)*nuc3[0][Cart::xxx][m+1];
      dip4[0][Cart::xxxz][_k][m] = PmB2*dip4[0][Cart::xxx][_k][m] - PmC2*dip4[0][Cart::xxx][_k][m+1] + (_k==2)*nuc3[0][Cart::xxx][m+1];
      dip4[0][Cart::xxyy][_k][m] = PmB0*dip4[0][Cart::xyy][_k][m] - PmC0*dip4[0][Cart::xyy][_k][m+1] + (_k==0)*nuc3[0][Cart::xyy][m+1] + term_yy;
      dip4[0][Cart::xxyz][_k][m] = PmB1*dip4[0][Cart::xxz][_k][m] - PmC1*dip4[0][Cart::xxz][_k][m+1] + (_k==1)*nuc3[0][Cart::xxz][m+1];
      dip4[0][Cart::xxzz][_k][m] = PmB0*dip4[0][Cart::xzz][_k][m] - PmC0*dip4[0][Cart::xzz][_k][m+1] + (_k==0)*nuc3[0][Cart::xzz][m+1] + term_zz;
      dip4[0][Cart::xyyy][_k][m] = PmB0*dip4[0][Cart::yyy][_k][m] - PmC0*dip4[0][Cart::yyy][_k][m+1] + (_k==0)*nuc3[0][Cart::yyy][m+1];
      dip4[0][Cart::xyyz][_k][m] = PmB0*dip4[0][Cart::yyz][_k][m] - PmC0*dip4[0][Cart::yyz][_k][m+1] + (_k==0)*nuc3[0][Cart::yyz][m+1];
      dip4[0][Cart::xyzz][_k][m] = PmB0*dip4[0][Cart::yzz][_k][m] - PmC0*dip4[0][Cart::yzz][_k][m+1] + (_k==0)*nuc3[0][Cart::yzz][m+1];
      dip4[0][Cart::xzzz][_k][m] = PmB0*dip4[0][Cart::zzz][_k][m] - PmC0*dip4[0][Cart::zzz][_k][m+1] + (_k==0)*nuc3[0][Cart::zzz][m+1];
      dip4[0][Cart::yyyy][_k][m] = PmB1*dip4[0][Cart::yyy][_k][m] - PmC1*dip4[0][Cart::yyy][_k][m+1] + (_k==1)*nuc3[0][Cart::yyy][m+1] + 3*term_yy;
      dip4[0][Cart::yyyz][_k][m] = PmB2*dip4[0][Cart::yyy][_k][m] - PmC2*dip4[0][Cart::yyy][_k][m+1] + (_k==2)*nuc3[0][Cart::yyy][m+1];
      dip4[0][Cart::yyzz][_k][m] = PmB1*dip4[0][Cart::yzz][_k][m] - PmC1*dip4[0][Cart::yzz][_k][m+1] + (_k==1)*nuc3[0][Cart::yzz][m+1] + term_zz;
      dip4[0][Cart::yzzz][_k][m] = PmB1*dip4[0][Cart::zzz][_k][m] - PmC1*dip4[0][Cart::zzz][_k][m+1] + (_k==1)*nuc3[0][Cart::zzz][m+1];
      dip4[0][Cart::zzzz][_k][m] = PmB2*dip4[0][Cart::zzz][_k][m] - PmC2*dip4[0][Cart::zzz][_k][m+1] + (_k==2)*nuc3[0][Cart::zzz][m+1] + 3*term_zz;
    }
  }
  //------------------------------------------------------

  //Integrals     p - g     d - g     f - g     g - g
  for (int m = 0; m < _lmax_col-3; m++) {
    for (int _i = 1; _i < n_orbitals[_lmax_row]; _i++) {
      int nx_i = nx[_i];
      int ny_i = ny[_i];
      int nz_i = nz[_i];
      int ilx_i = i_less_x[_i];
      int ily_i = i_less_y[_i];
      int ilz_i = i_less_z[_i];
      for (int _k = 0; _k < 3; _k++) {
        double term_xx = _fak*(dip4[_i][Cart::xx][_k][m]-dip4[_i][Cart::xx][_k][m+1]);
        double term_yy = _fak*(dip4[_i][Cart::yy][_k][m]-dip4[_i][Cart::yy][_k][m+1]);
        double term_zz = _fak*(dip4[_i][Cart::zz][_k][m]-dip4[_i][Cart::zz][_k][m+1]);
        dip4[_i][Cart::xxxx][_k][m] = PmB0*dip4[_i][Cart::xxx][_k][m] - PmC0*dip4[_i][Cart::xxx][_k][m+1] + (_k==0)*nuc3[_i][Cart::xxx][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::xxx][_k][m] - dip4[ilx_i][Cart::xxx][_k][m+1]) + 3*term_xx;
        dip4[_i][Cart::xxxy][_k][m] = PmB1*dip4[_i][Cart::xxx][_k][m] - PmC1*dip4[_i][Cart::xxx][_k][m+1] + (_k==1)*nuc3[_i][Cart::xxx][m+1]
                                      + ny_i*_fak*(dip4[ily_i][Cart::xxx][_k][m] - dip4[ily_i][Cart::xxx][_k][m+1]);
        dip4[_i][Cart::xxxz][_k][m] = PmB2*dip4[_i][Cart::xxx][_k][m] - PmC2*dip4[_i][Cart::xxx][_k][m+1] + (_k==2)*nuc3[_i][Cart::xxx][m+1]
                                      + nz_i*_fak*(dip4[ilz_i][Cart::xxx][_k][m] - dip4[ilz_i][Cart::xxx][_k][m+1]);
        dip4[_i][Cart::xxyy][_k][m] = PmB0*dip4[_i][Cart::xyy][_k][m] - PmC0*dip4[_i][Cart::xyy][_k][m+1] + (_k==0)*nuc3[_i][Cart::xyy][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::xyy][_k][m] - dip4[ilx_i][Cart::xyy][_k][m+1]) + term_yy;
        dip4[_i][Cart::xxyz][_k][m] = PmB1*dip4[_i][Cart::xxz][_k][m] - PmC1*dip4[_i][Cart::xxz][_k][m+1] + (_k==1)*nuc3[_i][Cart::xxz][m+1]
                                      + ny_i*_fak*(dip4[ily_i][Cart::xxz][_k][m] - dip4[ily_i][Cart::xxz][_k][m+1]);
        dip4[_i][Cart::xxzz][_k][m] = PmB0*dip4[_i][Cart::xzz][_k][m] - PmC0*dip4[_i][Cart::xzz][_k][m+1] + (_k==0)*nuc3[_i][Cart::xzz][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::xzz][_k][m] - dip4[ilx_i][Cart::xzz][_k][m+1]) + term_zz;
        dip4[_i][Cart::xyyy][_k][m] = PmB0*dip4[_i][Cart::yyy][_k][m] - PmC0*dip4[_i][Cart::yyy][_k][m+1] + (_k==0)*nuc3[_i][Cart::yyy][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::yyy][_k][m] - dip4[ilx_i][Cart::yyy][_k][m+1]);
        dip4[_i][Cart::xyyz][_k][m] = PmB0*dip4[_i][Cart::yyz][_k][m] - PmC0*dip4[_i][Cart::yyz][_k][m+1] + (_k==0)*nuc3[_i][Cart::yyz][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::yyz][_k][m] - dip4[ilx_i][Cart::yyz][_k][m+1]);
        dip4[_i][Cart::xyzz][_k][m] = PmB0*dip4[_i][Cart::yzz][_k][m] - PmC0*dip4[_i][Cart::yzz][_k][m+1] + (_k==0)*nuc3[_i][Cart::yzz][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::yzz][_k][m] - dip4[ilx_i][Cart::yzz][_k][m+1]);
        dip4[_i][Cart::xzzz][_k][m] = PmB0*dip4[_i][Cart::zzz][_k][m] - PmC0*dip4[_i][Cart::zzz][_k][m+1] + (_k==0)*nuc3[_i][Cart::zzz][m+1]
                                      + nx_i*_fak*(dip4[ilx_i][Cart::zzz][_k][m] - dip4[ilx_i][Cart::zzz][_k][m+1]);
        dip4[_i][Cart::yyyy][_k][m] = PmB1*dip4[_i][Cart::yyy][_k][m] - PmC1*dip4[_i][Cart::yyy][_k][m+1] + (_k==1)*nuc3[_i][Cart::yyy][m+1]
                                      + ny_i*_fak*(dip4[ily_i][Cart::yyy][_k][m] - dip4[ily_i][Cart::yyy][_k][m+1]) + 3*term_yy;
        dip4[_i][Cart::yyyz][_k][m] = PmB2*dip4[_i][Cart::yyy][_k][m] - PmC2*dip4[_i][Cart::yyy][_k][m+1] + (_k==2)*nuc3[_i][Cart::yyy][m+1]
                                      + nz_i*_fak*(dip4[ilz_i][Cart::yyy][_k][m] - dip4[ilz_i][Cart::yyy][_k][m+1]);
        dip4[_i][Cart::yyzz][_k][m] = PmB1*dip4[_i][Cart::yzz][_k][m] - PmC1*dip4[_i][Cart::yzz][_k][m+1] + (_k==1)*nuc3[_i][Cart::yzz][m+1]
                                      + ny_i*_fak*(dip4[ily_i][Cart::yzz][_k][m] - dip4[ily_i][Cart::yzz][_k][m+1]) + term_zz;
        dip4[_i][Cart::yzzz][_k][m] = PmB1*dip4[_i][Cart::zzz][_k][m] - PmC1*dip4[_i][Cart::zzz][_k][m+1] + (_k==1)*nuc3[_i][Cart::zzz][m+1]
                                      + ny_i*_fak*(dip4[ily_i][Cart::zzz][_k][m] - dip4[ily_i][Cart::zzz][_k][m+1]);
        dip4[_i][Cart::zzzz][_k][m] = PmB2*dip4[_i][Cart::zzz][_k][m] - PmC2*dip4[_i][Cart::zzz][_k][m+1] + (_k==2)*nuc3[_i][Cart::zzz][m+1]
                                      + nz_i*_fak*(dip4[ilz_i][Cart::zzz][_k][m] - dip4[ilz_i][Cart::zzz][_k][m+1]) + 3*term_zz;
      }
    }
  }
  //------------------------------------------------------

} // end if (_lmax_col > 3)

for (int _i = 0; _i < _nrows; _i++) {
  for (int _j = 0; _j < _ncols; _j++) {
    dip(_i,_j) = d_0 * dip4[_i][_j][0][0] + d_1 * dip4[_i][_j][1][0] + d_2 * dip4[_i][_j][2][0];
  }
}

        ub::matrix<double> _trafo_row = getTrafo(*itr);
        ub::matrix<double> _trafo_col_tposed = ub::trans(getTrafo(*itc));

        ub::matrix<double> _dip_tmp = ub::prod( _trafo_row, dip );
        ub::matrix<double> _dip_sph = ub::prod( _dip_tmp, _trafo_col_tposed );
        // save to _matrix

        for ( unsigned i = 0; i< _matrix.size1(); i++ ) {
            for (unsigned j = 0; j < _matrix.size2(); j++) {
                _matrix(i,j) += _dip_sph(i+_shell_row->getOffset(),j+_shell_col->getOffset());
            }
        }

            }// _shell_col Gaussians
        }// _shell_row Gaussians
        }

        void AODipole_Potential::Fillextpotential(const AOBasis& aobasis, const std::vector<ctp::PolarSeg*> & _sites) {

            _externalpotential = ub::zero_matrix<double>(aobasis.AOBasisSize(), aobasis.AOBasisSize());
            for (unsigned int i = 0; i < _sites.size(); i++) {
                for (ctp::PolarSeg::const_iterator it = _sites[i]->begin(); it < _sites[i]->end(); ++it) {

                    if ((*it)->getRank() > 0 || (*it)->IsPolarizable()) {
                        vec positionofsite = (*it)->getPos() * tools::conv::nm2bohr;
                        _aomatrix = ub::zero_matrix<double>(aobasis.AOBasisSize(), aobasis.AOBasisSize());
                        setAPolarSite((*it));
                        Fill(aobasis, positionofsite);
                        _externalpotential += _aomatrix;
                    }
                }
            }
            return;
        }

}}


#include <boost/test/unit_test.hpp>

#include <smtrat-mcsat/smtrat-mcsat.h>
#include <smtrat-mcsat/explanations/fm/Explanation.h>

using namespace smtrat;

BOOST_AUTO_TEST_SUITE(Test_Explanation_FM);

BOOST_AUTO_TEST_CASE(Test_One)
{

	carl::Variable a = carl::freshRealVariable("a");
	carl::Variable b = carl::freshRealVariable("b");

	// Original constraints
	FormulaT c1(Poly(b), carl::Relation::EQ);
	FormulaT c2(Poly(a)*b-Rational(1), carl::Relation::EQ);

	mcsat::Bookkeeping bk;
	bk.pushConstraint(c1);
	bk.pushConstraint(c2);
	mcsat::fm::Explanation<mcsat::fm::DefaultSettings> expl;

	{
		bk.pushAssignment(a, Rational(-1), FormulaT(VariableAssignmentT(a, Rational(-1))));
		auto ref = FormulaT(carl::FormulaType::OR, {
			c1.negated(),
			c2.negated(),
			FormulaT(ConstraintT(Poly(a), carl::Relation::GEQ))
		});
		auto res = expl(bk, b, {c1,c2});
		assert(res);
		std::cout << *res << std::endl;
		std::cout << "Should be " << ref << std::endl;
		BOOST_CHECK(boost::get<FormulaT>(&*res) != nullptr);
		BOOST_CHECK(ref == boost::get<FormulaT>(*res));
		bk.popAssignment(a);
	}
	{
		bk.pushAssignment(a, Rational(1), FormulaT(VariableAssignmentT(a, Rational(1))));
		auto ref = FormulaT(carl::FormulaType::OR, {
			c1.negated(),
			c2.negated(),
			FormulaT(ConstraintT(Poly(a), carl::Relation::LEQ))
		});
		std::cout << "Should be " << ref << std::endl;
		auto res = expl(bk, b, {c1,c2});
		assert(res);
		std::cout << *res << std::endl;
		BOOST_CHECK(boost::get<FormulaT>(&*res) != nullptr);
		BOOST_CHECK(ref == boost::get<FormulaT>(*res));
		bk.popAssignment(a);
	}
}

BOOST_AUTO_TEST_CASE(Test_Two)
{

	carl::Variable x = carl::freshRealVariable("x");
	carl::Variable y = carl::freshRealVariable("y");

	// Original constraints
	FormulaT c1(Poly(y), carl::Relation::GREATER);
	FormulaT c2(Poly(x) + Poly(y), carl::Relation::LESS);

	mcsat::Bookkeeping bk;
	bk.pushConstraint(c1);
	bk.pushConstraint(c2);
	mcsat::fm::Explanation<mcsat::fm::DefaultSettings> expl;

	{
		bk.pushAssignment(x, Rational(0), FormulaT(VariableAssignmentT(x, Rational(0))));
		auto res = expl(bk, y, {c1,c2});
		assert(res);
		std::cout << *res << std::endl;
	}
}

BOOST_AUTO_TEST_SUITE_END();


#include <fastad_bits/reverse/core/binary.hpp>
#include <fastad_bits/reverse/core/unary.hpp>
#include <fastad_bits/reverse/core/var.hpp>
#include <fastad_bits/reverse/core/eval.hpp>
#include <fastad_bits/reverse/core/eq.hpp>
#include <fastad_bits/reverse/core/pow.hpp>
#include <fastad_bits/reverse/core/sum.hpp>
#include <benchmark/benchmark.h>
#include <numeric>
#ifdef USE_ADEPT
#include <adept_arrays.h>
#endif

// Finite-difference method
static inline double f_test(const std::vector<double>& x)
{
    double sum = 0;
    for (const auto& xi : x) {
        sum += xi * xi;
    }
    return sum * sum + std::sin(sum);
}

static void BM_sumnode_fd(benchmark::State& state)
{
    constexpr double h = 1e-10;
    std::vector<double> x(10);
    for (size_t i = 0; i < x.size(); ++i) {
        x[i] = i;
    }
    for (auto _ : state) {
        double f = f_test(x);
        for (size_t i = 0; i < x.size(); ++i) {
            x[i] += h;
            double f_h = f_test(x);
            double dfdx_i = (f_h - f) / h;
            benchmark::DoNotOptimize(dfdx_i);
            x[i] -= h;
        }
    }
}

BENCHMARK(BM_sumnode_fd);

// FastAD
static void BM_sumnode_fastad(benchmark::State& state)
{
    using namespace ad;
    std::vector<Var<double>> vec;
    for (size_t i = 0; i < 1e1; ++i) {
        vec.emplace_back(i);
    }
    Var<double> w4, w5;
    auto sum_expr = ad::sum(vec.begin(), vec.end(), [](const auto& x) {return x * x;});
    auto expr = ad::bind((w4=sum_expr, w5 = w4 * w4 + ad::sin(w4)));

    for (auto _ : state) {
        autodiff(expr);
        benchmark::ClobberMemory();
    }
}

BENCHMARK(BM_sumnode_fastad);

static void BM_sumnode_fastad_large_vectorized(benchmark::State& state)
{
    using namespace ad;
    constexpr size_t size = 1000;
    std::vector<double> values(size);
    std::vector<double> values2(size);
    std::vector<Var<double>> w(2);
    w[0].get() = 2.;
    w[1].get() = 1.;

    for (size_t i = 0; i < size; ++i) {
        values[i] = values2[i] = i;
    }

    int i = 0;
    auto expr = ad::bind(
        ad::sum(values.begin(), values.end(),
            [&](double v) {
                if (i % values2.size() == 0) i = 0;
                auto&& expr = -ad::constant(0.5) *
                    ad::pow<2>((ad::constant(v) - w[0] * ad::constant(values2[i])) / w[1]);
                ++i;
                return expr;
            }));

    for (auto _ : state) {
        ad::autodiff(expr);
		benchmark::DoNotOptimize(expr);
    }
}

BENCHMARK(BM_sumnode_fastad_large_vectorized);

#ifdef USE_ADEPT

// Adept
static void BM_sumnode_adept(benchmark::State& state)
{
    using namespace adept;
    for (auto _ : state) {
        Stack stack;
        aVector x = {0.,1.,2.,3.,4.,5.,6.,7.,8.,9.};
        stack.new_recording();
        aReal y = sum(x * x);
        aReal J = pow(y, 2) + sin(y);
        J.set_gradient(1.);
        stack.reverse();
        benchmark::ClobberMemory();
    }
}

BENCHMARK(BM_sumnode_adept);

#endif