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

}}


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

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

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

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

#include <boost/make_shared.hpp>
#include <ql/instruments/impliedvolatility.hpp>
#include <ql/instruments/vanillaoption.hpp>
#include <ql/math/solvers1d/brent.hpp>
#include <ql/pricingengines/vanilla/analyticeuropeanengine.hpp>
#include <ql/processes/blackscholesprocess.hpp>
#include <ql/termstructures/volatility/equityfx/blackconstantvol.hpp>
#include <qle/pricingengines/baroneadesiwhaleyengine.hpp>
#include <qle/termstructures/blackvariancesurfacesparse.hpp>
#include <qle/termstructures/equityoptionsurfacestripper.hpp>

using std::pair;
using std::vector;
using namespace QuantLib;

namespace {

class PriceError {
public:
    PriceError(const VanillaOption& option, SimpleQuote& vol, Real targetValue)
        : option_(option), vol_(vol), targetValue_(targetValue){};
    Real operator()(Volatility x) const;

private:
    const VanillaOption& option_;
    SimpleQuote& vol_;
    Real targetValue_;
};

Real PriceError::operator()(Volatility x) const {
    vol_.setValue(x);
    Real npv;
    // Barone Adesi Whaley fails for very small variance, so wrap in a try/catch
    try {
        npv = option_.NPV();
    } catch (...) {
        npv = 0.0;
    }
    return npv - targetValue_;
}

} // namespace

namespace QuantExt {

EquityOptionSurfaceStripper::EquityOptionSurfaceStripper(const boost::shared_ptr<OptionInterpolatorBase>& callSurface,
                                                         const boost::shared_ptr<OptionInterpolatorBase>& putSurface,
                                                         const Handle<EquityIndex>& eqIndex, const Calendar& calendar,
                                                         const DayCounter& dayCounter, Exercise::Type type,
                                                         bool lowerStrikeConstExtrap, bool upperStrikeConstExtrap,
                                                         bool timeFlatExtrapolation)
    : callSurface_(callSurface), putSurface_(putSurface), eqIndex_(eqIndex), calendar_(calendar),
      dayCounter_(dayCounter), type_(type), lowerStrikeConstExtrap_(lowerStrikeConstExtrap),
      upperStrikeConstExtrap_(upperStrikeConstExtrap), timeFlatExtrapolation_(timeFlatExtrapolation) {

    // the call and put surfaces should have the same expiries/strikes/reference date/day counters, some checks to
    // ensure this
    QL_REQUIRE(callSurface_->referenceDate() == putSurface_->referenceDate(),
               "Mismatch between Call and Put reference dates in EquityOptionPremiumCurveStripper");

    // register with all market data
    registerWith(eqIndex);
    registerWith(Settings::instance().evaluationDate());
}

void EquityOptionSurfaceStripper::performCalculations() const {
    // create a set of all dates
    std::set<Date> allExpiries;
    std::vector<Date> callExpiries = callSurface_->expiries();
    std::vector<Date> putExpiries = putSurface_->expiries();
    for (auto expiry : callExpiries)
        allExpiries.insert(expiry);
    for (auto expiry : putExpiries)
        allExpiries.insert(expiry);

    boost::shared_ptr<SimpleQuote> volQuote;
    boost::shared_ptr<PricingEngine> engine;

    boost::shared_ptr<BlackVarianceSurfaceSparse> callVolSurface, putVolSurface;

    // if the surface is a price surface then we have premiums
    bool premiumSurfaces = boost::dynamic_pointer_cast<OptionPriceSurface>(callSurface_) != NULL;
    if (premiumSurfaces) {
        // first also check the put surface
        QL_REQUIRE(boost::dynamic_pointer_cast<OptionPriceSurface>(putSurface_) != NULL,
                   "Call price surface provided, but no put price surface");

        // Set up the engine for implying the vols
        // term structures needed to get implied vol
        volQuote = boost::make_shared<SimpleQuote>(0.1);
        Handle<BlackVolTermStructure> volTs(boost::make_shared<BlackConstantVol>(
            callSurface_->referenceDate(), calendar_, Handle<Quote>(volQuote), dayCounter_));

        // a black scholes process
        boost::shared_ptr<GeneralizedBlackScholesProcess> gbsp = boost::make_shared<BlackScholesMertonProcess>(
            eqIndex_->equitySpot(), eqIndex_->equityDividendCurve(), eqIndex_->equityForecastCurve(), volTs);

        // hard code the engines here
        // BaroneAdesiWhaley for American options - much faster than alternatives
        // Black for European
        if (type_ == Exercise::American) {
            engine = boost::make_shared<QuantExt::BaroneAdesiWhaleyApproximationEngine>(gbsp);
        } else if (type_ == Exercise::European) {
            engine = boost::make_shared<QuantExt::AnalyticEuropeanEngine>(gbsp);
        } else {
            QL_FAIL("Unsupported exercise type for option stripping");
        }
    } else {
        // we have variance surfaces, explicitly cast so we can look up vol later
        callVolSurface = boost::dynamic_pointer_cast<BlackVarianceSurfaceSparse>(callSurface_);
        putVolSurface = boost::dynamic_pointer_cast<BlackVarianceSurfaceSparse>(putSurface_);
    }

    vector<Real> volStrikes;
    vector<Real> volData;
    vector<Date> volExpiries;

    // loop over each expiry
    for (auto exp : allExpiries) {
        // get the forward price at time
        Real forward = eqIndex_->fixing(exp);

        vector<Real> callStrikes, putStrikes;
        auto itc = std::find(callExpiries.begin(), callExpiries.end(), exp);
        if (itc != callExpiries.end()) {
            auto pos = std::distance(callExpiries.begin(), itc);
            callStrikes = callSurface_->strikes().at(pos);
        }
        auto itp = std::find(putExpiries.begin(), putExpiries.end(), exp);
        if (itp != putExpiries.end()) {
            auto pos = std::distance(putExpiries.begin(), itp);
            putStrikes = putSurface_->strikes().at(pos);
        }

        // We want a set of prices both sides of ATM forward
        // We take calls where strike < atm, and puts where strike > atm

        bool haveCalls = false, havePuts = false;
        // check if calls in the correct range
        if (callStrikes.size() > 0 && callStrikes.front() < forward)
            haveCalls = true;

        // check if puts in the correct range
        if (putStrikes.size() > 0 && putStrikes.back() > forward)
            havePuts = true;

        for (auto cs : callStrikes) {
            if (!havePuts || cs < forward) {
                volStrikes.push_back(cs);
                volExpiries.push_back(exp);
                if (premiumSurfaces) {
                    volData.push_back(implyVol(exp, cs, Option::Call, engine, volQuote));
                } else {
                    volData.push_back(callVolSurface->blackVol(exp, cs));
                }
            }
        }
        for (auto ps : putStrikes) {
            if (!haveCalls || ps > forward) {
                volStrikes.push_back(ps);
                volExpiries.push_back(exp);
                if (premiumSurfaces) {
                    volData.push_back(implyVol(exp, ps, Option::Put, engine, volQuote));
                } else {
                    volData.push_back(putVolSurface->blackVol(exp, ps));
                }
            }
        }
    }
    volSurface_ = boost::make_shared<BlackVarianceSurfaceSparse>(
        callSurface_->referenceDate(), calendar_, volExpiries, volStrikes, volData, dayCounter_,
        lowerStrikeConstExtrap_, upperStrikeConstExtrap_, timeFlatExtrapolation_);
}

Real EquityOptionSurfaceStripper::implyVol(Date expiry, Real strike, Option::Type type,
                                           boost::shared_ptr<PricingEngine> engine,
                                           boost::shared_ptr<SimpleQuote> volQuote) const {

    // create an american option for current strike/expiry and type
    boost::shared_ptr<StrikedTypePayoff> payoff(new PlainVanillaPayoff(type, strike));
    boost::shared_ptr<Exercise> exercise;
    if (type_ == Exercise::American) {
        exercise = boost::make_shared<AmericanExercise>(expiry);
    } else if (type_ == Exercise::European) {
        exercise = boost::make_shared<EuropeanExercise>(expiry);
    } else {
        QL_FAIL("Unsupported exercise type for option stripping");
    }
    VanillaOption option(payoff, exercise);
    option.setPricingEngine(engine);

    // option.setPricingEngine(engine);
    Real targetPrice =
        type == Option::Call ? callSurface_->getValue(expiry, strike) : putSurface_->getValue(expiry, strike);

    // calculate the implied volatility using a solver
    Real vol;
    try {
        PriceError f(option, *volQuote, targetPrice);
        Brent solver;
        solver.setMaxEvaluations(100);
        solver.setLowerBound(0.0001);
        vol = solver.solve(f, 0.0001, 0.2, 0.01);
    } catch (...) {
        vol = 0.0;
    }
    return vol;
}

boost::shared_ptr<QuantLib::BlackVolTermStructure> EquityOptionSurfaceStripper::volSurface() {
    calculate();
    return volSurface_;
}

} // namespace QuantExt


/*
 * Author: Benoit Sklenard benoit.sklenard@cea.fr
 *
 *
 * Copyright 2014 IMDEA Materials Institute, Getafe, Madrid, Spain
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *    http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include <cmath>
#include <vector>

#include "io/Diagnostic.h"
#include "io/ParameterManager.h"

#include "okmc/Defect.h"
#include "kernel/ParticleType.h"
#include "okmc/MobileParticleParam.h"

#include <boost/numeric/ublas/io.hpp>

#include "kernel/Mesh.h"

#include "ParticleToNodeHandler.h"

#include <ctime>

using namespace Electrostatics;
using namespace Kernel;
using namespace boost::numeric;

ParticleToNodeHandler::ParticleToNodeHandler(Kernel::Domain *pDomain, Kernel::Mesh *pMesh) {
	LOWMSG("Loading Particle To Node handler");
	_pMesh   = pMesh;
	_pDomain = pDomain;
}

ParticleToNodeHandler::~ParticleToNodeHandler() {

}

inline double ParticleToNodeHandler::getOverlap(double u1, double u2) {
	return 15./16. * ((u2 - u1) - 2./3. * (u2*u2*u2 - u1*u1*u1) + 1./5. * (u2*u2*u2*u2*u2 - u1*u1*u1*u1*u1));
}

void ParticleToNodeHandler::remove(OKMC::Particle *pPart) {
	std::map<OKMC::Particle *, std::set<MeshNode *> >::iterator mit;

	mit = _syncOKMC.find(pPart);
	if (mit != _syncOKMC.end()) {
		for (std::set<MeshNode *>::iterator sit = mit->second.begin(); sit != mit->second.end(); ++sit)
			(*sit)->remove(pPart);
		_syncOKMC.erase(mit);
	}
}

void ParticleToNodeHandler::remove(LKMC::LatticeAtom *pLA) {
	std::map<LKMC::LatticeAtom *, std::set<MeshNode *> >::iterator mit;

	mit = _syncLKMC.find(pLA);
	if (mit != _syncLKMC.end()) {
		for (std::set<MeshNode *>::iterator sit = mit->second.begin(); sit != mit->second.end(); ++sit)
			(*sit)->remove(pLA);
		_syncLKMC.erase(mit);
	}
}

double ParticleToNodeHandler::addWeightNode(MeshNode *pNode, LKMC::LatticeAtom *pLA) {
	M_TYPE mt = pLA->getElement()->getMaterial();
	double dr = _pDomain->_pLaPar[mt]->_orbitalRadius;

	ublas::vector<double> n  = ublas::zero_vector<double>(3);
	ublas::vector<double> c1 = ublas::zero_vector<double>(3);
	ublas::vector<double> c2 = ublas::zero_vector<double>(3);
	MeshNode ***pNodes = _pMesh->getNodes();

	n(0)  = pLA->getCoordinates()._x;
	n(1)  = pLA->getCoordinates()._y;
	n(2)  = pLA->getCoordinates()._z;

	c1(0) = pNode->_xm;
	c1(1) = pNode->_ym;
	c1(2) = pNode->_zm;
	c2(0) = pNode->_xp;
	c2(1) = pNode->_yp;
	c2(2) = pNode->_zp;

	_pMesh->setPeriodicRelative(n, c1); // c1 = c1 - n
	_pMesh->setPeriodicRelative(n, c2); // c2 = c2 - n

	c1 /= dr;
	c2 /= dr;

	if (c1(0) <= -1) c1(0) = -1.;
	if (c2(0) <= -1) c2(0) = -1.;
	if (c1(0) >= 1)  c1(0) = 1.;
	if (c2(0) >= 1)  c2(0) = 1.;

	if (c1(1) <= -1) c1(1) = -1.;
	if (c2(1) <= -1) c2(1) = -1.;
	if (c1(1) >= 1)  c1(1) = 1.;
	if (c2(1) >= 1)  c2(1) = 1.;

	if (c1(2) <= -1) c1(2) = -1.;
	if (c2(2) <= -1) c2(2) = -1.;
	if (c1(2) >= 1)  c1(2) = 1.;
	if (c2(2) >= 1)  c2(2) = 1.;

	const double wx  = getOverlap(c1(0), c2(0));
	const double wy  = getOverlap(c1(1), c2(1));
	const double wz  = getOverlap(c1(2), c2(2));

	const double w   = wx * wy * wz;

	if (w > 0) {
		unsigned i = pNode->_ix;
		unsigned j = pNode->_iy;
		unsigned k = pNode->_iz;

		if (_pMesh->getPeriodicX() && pNode->_ix == (_pMesh->getnx() - 1))
			i = 0;
		if (_pMesh->getPeriodicY() && pNode->_iy == (_pMesh->getny() - 1))
			j = 0;
		if (_pMesh->getPeriodicZ() && pNode->_iz == (_pMesh->getnz() - 1))
			k = 0;

		pNodes[i][j][k].insert(pLA, w);

	    std::pair<std::map<LKMC::LatticeAtom *, std::set<MeshNode *> >::iterator, bool > r;
	    r = _syncLKMC.insert(std::pair<LKMC::LatticeAtom *, std::set<MeshNode *> >(pLA, std::set<MeshNode *>()));
	    r.first->second.insert(&pNodes[i][j][k]);
	}

	return w;
}

double ParticleToNodeHandler::addWeightNode(MeshNode *pNode, OKMC::Particle *pPart) {
	M_TYPE mt = pPart->getElement()->getMaterial();
	P_TYPE pt = pPart->getPType();

	double dr = _pDomain->_pMPPar->_orbitalRadius[mt][pt];
	// double dr = 1.; // _pDomain->_pLaPar[mt]->_orbitalRadius;

	ublas::vector<double> n  = ublas::zero_vector<double>(3);
	ublas::vector<double> c1 = ublas::zero_vector<double>(3);
	ublas::vector<double> c2 = ublas::zero_vector<double>(3);
	MeshNode ***pNodes = _pMesh->getNodes();

	n(0)  = pPart->getCoordinates()._x;
	n(1)  = pPart->getCoordinates()._y;
	n(2)  = pPart->getCoordinates()._z;

	c1(0) = pNode->_xm;
	c1(1) = pNode->_ym;
	c1(2) = pNode->_zm;
	c2(0) = pNode->_xp;
	c2(1) = pNode->_yp;
	c2(2) = pNode->_zp;

	_pMesh->setPeriodicRelative(n, c1); // c1 = c1 - n
	_pMesh->setPeriodicRelative(n, c2); // c2 = c2 - n

	c1 /= dr;
	c2 /= dr;

	if (c1(0) <= -1) c1(0) = -1.;
	if (c2(0) <= -1) c2(0) = -1.;
	if (c1(0) >= 1)  c1(0) = 1.;
	if (c2(0) >= 1)  c2(0) = 1.;

	if (c1(1) <= -1) c1(1) = -1.;
	if (c2(1) <= -1) c2(1) = -1.;
	if (c1(1) >= 1)  c1(1) = 1.;
	if (c2(1) >= 1)  c2(1) = 1.;

	if (c1(2) <= -1) c1(2) = -1.;
	if (c2(2) <= -1) c2(2) = -1.;
	if (c1(2) >= 1)  c1(2) = 1.;
	if (c2(2) >= 1)  c2(2) = 1.;

	const double wx  = getOverlap(c1(0), c2(0));
	const double wy  = getOverlap(c1(1), c2(1));
	const double wz  = getOverlap(c1(2), c2(2));

	const double w   = wx * wy * wz;

	if (w > 0) {
		unsigned i = pNode->_ix;
		unsigned j = pNode->_iy;
		unsigned k = pNode->_iz;

		if (_pMesh->getPeriodicX() && pNode->_ix == (_pMesh->getnx() - 1))
			i = 0;
		if (_pMesh->getPeriodicY() && pNode->_iy == (_pMesh->getny() - 1))
			j = 0;
		if (_pMesh->getPeriodicZ() && pNode->_iz == (_pMesh->getnz() - 1))
			k = 0;

		pNodes[i][j][k].insert(pPart, w);

	    std::pair<std::map<OKMC::Particle *, std::set<MeshNode *> >::iterator, bool > r;
	    r = _syncOKMC.insert(std::pair<OKMC::Particle *, std::set<MeshNode *> >(pPart, std::set<MeshNode *>()));
	    r.first->second.insert(&pNodes[i][j][k]);
	}

	return w;
}

void ParticleToNodeHandler::insert(LKMC::LatticeAtom *pLA) {
	std::set<MeshNode*> MN;
	MeshNode* pMN = NULL;

	M_TYPE mt = pLA->getElement()->getMaterial();

	double dr = _pDomain->_pLaPar[mt]->_orbitalRadius;

	remove(pLA);
	MeshNode ***pNodes  = _pMesh->getNodes();
	pMN = _pMesh->getFirstNodeFromElement(pLA->getElement());

	const size_t dx = ceil(dr / fabs(pNodes[0][0][0]._xm - pNodes[0][0][0]._xp)) + 1;
	const size_t dy = ceil(dr / fabs(pNodes[0][0][0]._ym - pNodes[0][0][0]._yp)) + 1;
	const size_t dz = ceil(dr / fabs(pNodes[0][0][0]._zm - pNodes[0][0][0]._zp)) + 1;

	const size_t startx = (dx <= pMN->_ix ? pMN->_ix - dx : _pMesh->getnx() - 1 - dx + pMN->_ix);
	const size_t endx   = (pMN->_ix + dx) % _pMesh->getnx();
	const size_t starty = (dy <= pMN->_iy ? pMN->_iy - dy : _pMesh->getny() - 1 - dy + pMN->_iy);
	const size_t endy   = (pMN->_iy + dy) % _pMesh->getny();
	const size_t startz = (dz <= pMN->_iz ? pMN->_iz - dz : _pMesh->getnz() - 1 - dz + pMN->_iz);
	const size_t endz   = (pMN->_iz + dz) % _pMesh->getnz();

	double w = 0.;
	for (size_t ix = startx; ix != endx; ix = (ix + 1) % _pMesh->getnx()) {
		for (size_t iy = starty; iy != endy; iy = (iy + 1) % _pMesh->getny()) {
			for (size_t iz = startz; iz != endz; iz = (iz + 1) % _pMesh->getnz()) {
				w += addWeightNode(&pNodes[ix][iy][iz], pLA);
			}
		}
	}

	if (_syncLKMC.find(pLA) ==_syncLKMC.end())
		ERRORMSG("LatticeAtom has not been inserted!");
}

void ParticleToNodeHandler::insert(OKMC::Particle *pPart) {
	std::set<MeshNode*> MN;
	MeshNode* pMN = NULL;

	M_TYPE mt = pPart->getElement()->getMaterial();
	P_TYPE pt = pPart->getPType();

	if (!_pDomain->_pMPPar->_mapToGrid[mt][pt]) {
	//	WARNINGMSG("Not inserted (OKMC) " << Domains::global()->PM()->getMaterialName(mt) << " " << Domains::global()->PM()->getParticleName(mt, pt));
		return ;
	}

	double dr = _pDomain->_pMPPar->_orbitalRadius[mt][pt];

	remove(pPart);
	MeshNode ***pNodes  = _pMesh->getNodes();
	pMN = _pMesh->getFirstNodeFromElement(pPart->getElement());

	const size_t dx = ceil(dr / fabs(pNodes[0][0][0]._xm - pNodes[0][0][0]._xp)) + 1;
	const size_t dy = ceil(dr / fabs(pNodes[0][0][0]._ym - pNodes[0][0][0]._yp)) + 1;
	const size_t dz = ceil(dr / fabs(pNodes[0][0][0]._zm - pNodes[0][0][0]._zp)) + 1;

	const size_t startx = (dx <= pMN->_ix ? pMN->_ix - dx : _pMesh->getnx() - 1 - dx + pMN->_ix);
	const size_t endx   = (pMN->_ix + dx) % _pMesh->getnx();
	const size_t starty = (dy <= pMN->_iy ? pMN->_iy - dy : _pMesh->getny() - 1 - dy + pMN->_iy);
	const size_t endy   = (pMN->_iy + dy) % _pMesh->getny();
	const size_t startz = (dz <= pMN->_iz ? pMN->_iz - dz : _pMesh->getnz() - 1 - dz + pMN->_iz);
	const size_t endz   = (pMN->_iz + dz) % _pMesh->getnz();

	double w = 0.;
	for (size_t ix = startx; ix != endx; ix = (ix + 1) % _pMesh->getnx()) {
		for (size_t iy = starty; iy != endy; iy = (iy + 1) % _pMesh->getny()) {
			for (size_t iz = startz; iz != endz; iz = (iz + 1) % _pMesh->getnz()) {
				w += addWeightNode(&pNodes[ix][iy][iz], pPart);
			}
		}
	}

	if (_syncOKMC.find(pPart) ==_syncOKMC.end())
		ERRORMSG("Particle has not been inserted!");
}

void ParticleToNodeHandler::getParticleNodes(OKMC::Particle *pPart, std::set<MeshNode *> &nodes) {
	std::map<OKMC::Particle *, std::set<MeshNode *> >::iterator mit = _syncOKMC.find(pPart);

	if (mit != _syncOKMC.end())
		for (std::set<MeshNode *>::iterator sit = mit->second.begin(); sit != mit->second.end(); ++sit)
			nodes.insert(*sit);
}

void ParticleToNodeHandler::getParticleNodes(LKMC::LatticeAtom *pLA, std::set<MeshNode *> &nodes) {
	std::map<LKMC::LatticeAtom *, std::set<MeshNode *> >::iterator mit = _syncLKMC.find(pLA);

	if (mit != _syncLKMC.end())
		for (std::set<MeshNode *>::iterator sit = mit->second.begin(); sit != mit->second.end(); ++sit)
			nodes.insert(*sit);
}

void ParticleToNodeHandler::print() {
	LOWMSG(_syncOKMC.size() << " particles in ParticleToNodeHandler:");

	if (!_syncOKMC.empty()) {
		unsigned i = 0;

		for (std::map<OKMC::Particle *, std::set<MeshNode *> >::iterator mit = _syncOKMC.begin(); mit != _syncOKMC.end(); ++mit) {
			double w = 0.;
			for (std::set<MeshNode *>::iterator sit = mit->second.begin(); sit != mit->second.end(); ++sit) {
				std::map<OKMC::Particle *, double>::iterator mit2 = (*sit)->_mPart.find(mit->first);
				if (mit2 != (*sit)->_mPart.end())
					w += mit2->second;
				else
					LOWMSG("Got a null value!");
			}
			LOWMSG("Particle " << i++ << " " <<  mit->first->getCoordinates() << " is affected to " << mit->second.size() << " nodes" << " weight= " << w);
		}
	}
}

void ParticleToNodeHandler::printLKMC() {
	LOWMSG(_syncLKMC.size() << " LKMC::LatticeAtom in ParticleToNodeHandler:");

	if (!_syncLKMC.empty()) {
		unsigned i = 0;

		for (std::map<LKMC::LatticeAtom *, std::set<MeshNode *> >::iterator mit = _syncLKMC.begin(); mit != _syncLKMC.end(); ++mit) {
			double w = 0.;
			for (std::set<MeshNode *>::iterator sit = mit->second.begin(); sit != mit->second.end(); ++sit) {
				std::map<LKMC::LatticeAtom *, double>::iterator mit2 = (*sit)->_mLA.find(mit->first);
				if (mit2 != (*sit)->_mLA.end())
					w += mit2->second;
				else
					LOWMSG("Got a null value!");
			}
				LOWMSG("LKMC::LaticeAtom " << ++i << " " <<  mit->first->getCoordinates() << " is affected to " << mit->second.size() << " nodes" << " weight= " << w);
		}
	}
}