//
// edip_c.cpp
//
// LGPL Version 2.1 HEADER START
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
//
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
// MA 02110-1301  USA
//
// LGPL Version 2.1 HEADER END
//

//
// Copyright (c) 2021, Regents of the University of Minnesota.
// All rights reserved.
//
// Contributors:
//    Yaser Afshar
//

#include "edip_c.hpp"

#include <cmath>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <memory>
#include <string>

#include "helper.hpp"
#include "special.hpp"
#include "utility.hpp"

namespace edip_c {
static constexpr std::size_t kDelta{16384};  // 2**14
static constexpr std::size_t kDeltaN{1024};  // 2**10

/*!
 * \brief EDIP/C number of parameters per line.
 *
 * EDIP/C parameter file. One set of params can span multiple lines.
 * Only store the 1st entry.
 *
 * format of an entry (one or more lines) in a parameter file
 *
 *     eps   BB       beta         sigma  a      aprime
 *     Z0    lambda0  lambdaprime  gamma  q
 *     flow  fhigh    alpha        plow   phigh  Zdih  Zrep  c0
 *     zbl   cutoff   skin         shift
 *
 * if zbl is 0, then \c cutoff, \c skin, \c shift are not necessary.
 *
 * units for each parameters:
 *
 *     eps, lambda0, Zdih, Zrep -> are in eV
 *     sigma, a, aprime, gamma, flow, fhigh, plow, phigh, c0 -> are in Angstrom
 *     cutoff, skin, shift -> are in Angstrom
 *     BB -> is in Angstrom^4
 *     beta, Z0, lambdaprime, q, alpha, -> are pure numbers
 *     zbl -> is 0 or 1
 *
 */
static constexpr int kParamsPerLine{20};
static constexpr int kMaxParamsPerLine{23};

// NOTE:
// t1 & t2 differ from the values in the paper where t1=10 and t2=2.5.

/*! tau constants t1 & t2 */
static constexpr double t1{-6.0 * 2.5};
static constexpr double t2{6.0};

static constexpr double kOneTwelve{1.0 / 12.0};
}  // namespace edip_c

int EDIPC::ProcessParameterFile(std::FILE *const parameter_file_pointer,
                                int const max_line_size) {
  std::unique_ptr<char> next_line_(new char[max_line_size]);
  char *next_line = next_line_.get();

  // Create a utility object
  Utility ut;

  // Read each set of parameters from the EDIP/C parameter file.
  // One set of params can span multiple lines. Only store the 1st entry.
  char *words[edip_c::kMaxParamsPerLine + 1];

  if (ut.GetNextLine(parameter_file_pointer, next_line, max_line_size)) {
    HELPER_LOG_ERROR(
        "End of file while reading a line from the `edip/c` parameter file.");
    return true;
  }

  int nwords = ut.GetNumberOfWordsInLine(next_line);

  // concatenate additional lines until have kParamsPerLine words
  while (nwords < edip_c::kParamsPerLine) {
    std::size_t const n = std::strlen(next_line);

    if (ut.GetNextLine(parameter_file_pointer, &next_line[n],
                       max_line_size - static_cast<int>(n))) {
      std::string msg{"End of file while reading a line from the `edip/c` "};
      msg += "parameter file.\nIncorrect format in the `edip/c` parameter ";
      msg += "file.\n" + std::to_string(nwords) + " parameters are provided ";
      msg += "while the model expects 20 parameters if zbl is 0 and 23 if ";
      msg += "it is 1.";
      HELPER_LOG_ERROR(msg);
      return true;
    }

    nwords = ut.GetNumberOfWordsInLine(next_line);
  }  // nwords < kParamsPerLine

  if (nwords != edip_c::kParamsPerLine && nwords != edip_c::kMaxParamsPerLine) {
    std::string msg{"Incorrect format in the `edip/c` parameter file.\n"};
    msg += std::to_string(nwords) + " parameters are provided while the ";
    msg += "code expects 20 parameters if zbl is 0 and 23 if it is 1.";
    HELPER_LOG_ERROR(msg);
    return true;
  }

  nwords = 0;
  words[nwords++] = std::strtok(next_line, "' \t\n\r\f");
  while ((words[nwords] = std::strtok(nullptr, "' \t\n\r\f"))) {
    ++nwords;
  }

  // format of an entry (one or more lines) in a parameter file
  //
  //     eps   BB       beta         sigma  a      aprime
  //     Z0    lambda0  lambdaprime  gamma  q
  //     flow  fhigh    alpha        plow   phigh  Zdih  Zrep  c0
  //     zbl   cutoff   skin         shift
  //
  // if zbl is 0, then cutoff, skin, shift are not necessary.
  //
  // units for each parameters:
  //
  //     eps, lambda0, Zdih, Zrep -> are in eV
  //     sigma, a, aprime, gamma, flow, fhigh, plow, phigh, c0 -> are in
  //     Angstrom cutoff, skin, shift -> are in Angstrom BB -> is in Angstrom^4
  //     beta, Z0, lambdaprime, q, alpha, -> are pure numbers
  //     zbl -> is 0 or 1
  //

  for (int i = 0; i < edip_c::kParamsPerLine - 1; ++i) {
    if (!ut.IsDouble(words[i])) {
      std::string msg(words[i]);
      msg += " is not a valid floating-point number.";
      HELPER_LOG_ERROR(msg);
      return true;
    }
  }  // Loop over i

  // Two-body
  params_.eps = std::atof(words[0]);
  params_.BB = std::atof(words[1]);
  params_.beta = std::atof(words[2]);
  params_.sigma = std::atof(words[3]);
  params_.a = std::atof(words[4]);
  params_.aprime = std::atof(words[5]);

  // Three-body
  params_.Z0 = std::atof(words[6]);
  params_.lambda0 = std::atof(words[7]);
  params_.lambdaprime = std::atof(words[8]);
  params_.gamma = std::atof(words[9]);
  params_.q = std::atof(words[10]);

  // Coordination parameters
  params_.flow = std::atof(words[11]);
  params_.fhigh = std::atof(words[12]);

  params_.alpha = std::atof(words[13]);

  params_.plow = std::atof(words[14]);
  params_.phigh = std::atof(words[15]);

  params_.Zdih = std::atof(words[16]);
  params_.Zrep = std::atof(words[17]);
  params_.c0 = std::atof(words[18]);

  if (ut.IsInteger(words[19])) {
    int const lzbl = std::atoi(words[19]);
    if (lzbl != 0 && lzbl != 1) {
      HELPER_LOG_ERROR(
          "Incorrect format in the `edip/c` parameter file.\n"
          "zbl must be 0 or 1.");
      return true;
    }
    lzbl_ = static_cast<bool>(lzbl);
  } else {
    std::string msg(words[19]);
    msg += " is not a valid integer number.\nzbl must be 0 or 1.";
    HELPER_LOG_ERROR(msg);
    return true;
  }

  if (lzbl_) {
    if (nwords != edip_c::kMaxParamsPerLine) {
      std::rewind(parameter_file_pointer);

      if (ut.GetNextLine(parameter_file_pointer, next_line, max_line_size)) {
        HELPER_LOG_ERROR(
            "End of file while reading a line from the "
            "`edip/c` parameter file.");
        return true;
      }

      nwords = ut.GetNumberOfWordsInLine(next_line);

      // concatenate additional lines until have kMaxParamsPerLine words
      while (nwords < edip_c::kMaxParamsPerLine) {
        std::size_t const n = std::strlen(next_line);

        if (ut.GetNextLine(parameter_file_pointer, &next_line[n],
                           max_line_size - static_cast<int>(n))) {
          HELPER_LOG_ERROR(
              "End of file while reading a line from the `edip/c` parameter "
              "file.");
          return true;
        }

        nwords = ut.GetNumberOfWordsInLine(next_line);
      }  // nwords < kMaxParamsPerLine

      if (nwords != edip_c::kMaxParamsPerLine) {
        HELPER_LOG_ERROR("Incorrect format in the `edip/c` parameter file.");
        return true;
      }

      nwords = 0;
      words[nwords++] = std::strtok(next_line, "' \t\n\r\f");
      while ((words[nwords] = std::strtok(nullptr, "' \t\n\r\f"))) {
        ++nwords;
      }
    }  // nwords != kMaxParamsPerLine

    for (int i = edip_c::kParamsPerLine; i < edip_c::kMaxParamsPerLine; ++i) {
      if (ut.IsDouble(words[i])) {
        continue;
      }
      std::string msg(words[i]);
      msg += " is not a valid floating-point number.";
      HELPER_LOG_ERROR(msg);
      return true;
    }  // Loop over i

    zbl_.reset(new ZBL());

    double cutoff = std::atof(words[20]);
    double skin = std::atof(words[21]);
    double shift = std::atof(words[22]);

    fermi_.reset(new FERMI(cutoff, skin, shift));
  }  // lzbl_

  // Everything is good
  return false;
}

void EDIPC::ConvertUnit(double const convert_length_factor,
                        double const convert_energy_factor) {
  // (Angstroms in metal units).
  if (special::IsNotOne(convert_length_factor)) {
    convert_length_factor_ = convert_length_factor;

    // sigma, a, aprime, gamma, flow, fhigh, plow, phigh, c0 -> are in Angstrom
    params_.sigma *= convert_length_factor;
    params_.a *= convert_length_factor;
    params_.aprime *= convert_length_factor;
    params_.gamma *= convert_length_factor;
    params_.flow *= convert_length_factor;
    params_.fhigh *= convert_length_factor;
    params_.plow *= convert_length_factor;
    params_.phigh *= convert_length_factor;
    params_.c0 *= convert_length_factor;
    params_.skin *= convert_length_factor;

    // BB -> is in Angstrom^4
    double const cst2 = convert_length_factor * convert_length_factor;
    double const cst4 = cst2 * cst2;
    params_.BB *= cst4;
  }

  //  (eV in the case of metal units).
  if (special::IsNotOne(convert_energy_factor)) {
    convert_energy_factor_ = convert_energy_factor;

    // eps, lambda0, Zdih, Zrep -> are in eV
    params_.eps *= convert_energy_factor;
    params_.lambda0 *= convert_energy_factor;
    params_.Zdih *= convert_energy_factor;
    params_.Zrep *= convert_energy_factor;
  }

  if (lzbl_) {
    zbl_->ConvertUnit(convert_length_factor, convert_energy_factor);
    // cutoff, skin, shift -> are in Angstrom
    fermi_->ConvertUnit(convert_length_factor, convert_energy_factor);
  }
}

void EDIPC::Resize(std::size_t const number_of_particles,
                   std::size_t const total_number_of_neighbors) {
  if (number_of_particles > number_of_neighbors_.size()) {
    std::size_t const max_number_of_particles =
        number_of_particles / edip_c::kDelta * edip_c::kDelta + edip_c::kDelta;

    // Number of neighbors for each atom
    number_of_neighbors_.resize(max_number_of_particles);
    neighbors_offset_.resize(max_number_of_particles);

    // Spherical coordination for each atom
    z_.resize(max_number_of_particles);

    // Switching functions for each atom
    pi_.resize(max_number_of_particles);
    dpi_.resize(max_number_of_particles);
    pi2_.resize(max_number_of_particles);
    dpi2_.resize(max_number_of_particles);
    pi3_.resize(max_number_of_particles);
    dpi3_.resize(max_number_of_particles);

    // Generalized coordination for each atom
    Z_.resize(max_number_of_particles);
  }

  if (total_number_of_neighbors > neighbors_to_particles_.size()) {
    std::size_t const max_total_number_of_neighbors =
        total_number_of_neighbors / edip_c::kDeltaN * edip_c::kDeltaN +
        edip_c::kDeltaN;

    neighbors_to_particles_.resize(max_total_number_of_neighbors);

    r_.resize(max_total_number_of_neighbors);
    N_.resize(max_total_number_of_neighbors * 3);

    dz_.resize(max_total_number_of_neighbors);
    p_.resize(max_total_number_of_neighbors);
    dp_.resize(max_total_number_of_neighbors);
  }
}

void EDIPC::Resize(std::size_t const number_of_particles) {
  std::size_t const max_number_of_neighbors =
      *std::max_element(number_of_neighbors_.data(),
                        number_of_neighbors_.data() + number_of_particles);

  if (max_number_of_neighbors > finiteforce_.size()) {
    finiteforce_.resize(max_number_of_neighbors);
    finiteforce2_.resize(max_number_of_neighbors, max_number_of_neighbors);

    Dzdx_.resize(max_number_of_neighbors * 3);
    Dzdxx_.resize(max_number_of_neighbors, max_number_of_neighbors * 3);
  }
}

void EDIPC::CompleteSetup(double *max_cutoff) {
  // Maximum cutoff distance
  *max_cutoff = params_.c0 + params_.skin;
}

void EDIPC::CutoffFunction(std::size_t const number_of_particles) {
  double const flow = params_.flow;
  double const fhigh = params_.fhigh;
  double const flow1 = flow + 0.001;
  double const fhigh1 = fhigh - 0.001;
  double const alpha = params_.alpha;
  double const plow = params_.plow;
  double const phigh = params_.phigh;
  double const plow1 = plow + 0.001;
  double const phigh1 = phigh - 0.001;
  double const dxf = 1.0 / (fhigh - flow);
  double const dxp = 1.0 / (phigh - plow);

  auto r_i = &r_[0];
  auto dz_i = &dz_[0];
  auto p_i = &p_[0];
  auto dp_i = &dp_[0];

  for (std::size_t i = 0; i < number_of_particles; ++i) {
    std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
    // Calculate f(r_ij) [--> z(i)]
    z_[i] = 0.0;

    for (std::size_t nn = 0; nn < number_of_neighbors_i; ++nn) {
      if (r_i[nn] < flow1) {
        z_[i] += 1.0;
        dz_i[nn] = 0.0;
      } else if (r_i[nn] > fhigh1) {
        dz_i[nn] = 0.0;
      } else {
        double const x = (r_i[nn] - flow) * dxf;
        double const invx = 1.0 / x;
        double const invx2 = invx * invx;
        double const invx3 = invx2 * invx;
        double const frac = 1.0 / (1.0 - invx3);
        double const dinvx = -invx2 * dxf;
        double const dinvx3 = 3.0 * invx2 * dinvx;
        double const dfrac = frac * frac * dinvx3;
        double const f = std::exp(alpha * frac);

        z_[i] += f;
        dz_i[nn] = f * alpha * dfrac;
      }  // end if
    }    // Loop over nn

    dz_i += number_of_neighbors_i;

    // Calculate p(r_ij)
    for (std::size_t nn = 0; nn < number_of_neighbors_i; ++nn) {
      if (r_i[nn] < plow1) {
        p_i[nn] = 1.0;
        dp_i[nn] = 0.0;
      } else if (r_i[nn] > phigh1) {
        p_i[nn] = 0.0;
        dp_i[nn] = 0.0;
      } else {
        double const x = (r_i[nn] - plow) * dxp;
        double const invx = 1.0 / x;
        double const invx2 = invx * invx;
        double const invx3 = invx2 * invx;
        double const frac = 1.0 / (1.0 - invx3);
        double const dinvx = -invx2 * dxp;
        double const dinvx3 = 3.0 * invx2 * dinvx;
        double const dfrac = frac * frac * dinvx3;
        double const f = std::exp(alpha * frac);

        p_i[nn] = f;
        dp_i[nn] = f * alpha * dfrac;
      }  // end if
    }    // Loop over nn

    r_i += number_of_neighbors_i;
    p_i += number_of_neighbors_i;
    dp_i += number_of_neighbors_i;
  }  // Loop over i
}

void EDIPC::SwitchFunction(std::size_t const number_of_particles) {
  for (std::size_t i = number_of_particles; i--;) {
    double const zi = z_[i];
    if (zi > 4.0) {
      pi_[i] = 0.0;
      dpi_[i] = 0.0;
      pi2_[i] = 0.0;
      dpi2_[i] = 0.0;
      pi3_[i] = 0.0;
      dpi3_[i] = 0.0;
    } else if (zi > 3.0) {
      double const a3 = zi - 3.0;
      double const a3a = a3 * a3 - 1.0;
      double const a6a = a3a * a3a;
      double const a4a = 4.0 * a3 * a3a;
      pi_[i] = a6a;
      dpi_[i] = a4a;
      pi2_[i] = 0.0;
      dpi2_[i] = 0.0;
      pi3_[i] = a6a;
      dpi3_[i] = a4a;
    } else if (zi > 2.0) {
      double const a2 = zi - 2.0;
      double const a3 = zi - 3.0;
      double const a2a = a2 * a2 - 1.0;
      double const a3a = a3 * a3 - 1.0;
      pi_[i] = 1.0;
      dpi_[i] = 0.0;
      pi2_[i] = a2a * a2a;
      dpi2_[i] = 4.0 * a2 * a2a;
      pi3_[i] = a3a * a3a;
      dpi3_[i] = 4.0 * a3 * a3a;
    } else if (zi > 1.0) {
      double const a2 = zi - 2.0;
      double const a2a = a2 * a2 - 1.0;
      pi_[i] = 1.0;
      dpi_[i] = 0.0;
      pi2_[i] = a2a * a2a;
      dpi2_[i] = 4.0 * a2 * a2a;
      pi3_[i] = 0.0;
      dpi3_[i] = 0.0;
    } else {
      pi_[i] = 1.0;
      dpi_[i] = 0.0;
      pi2_[i] = 0.0;
      dpi2_[i] = 0.0;
      pi3_[i] = 0.0;
      dpi3_[i] = 0.0;
    }
  }  // Loop over i
}

void EDIPC::Coordination(std::size_t const i) {
  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  if (number_of_neighbors_i == 0) {
    return;
  }

  // Initialize general coordination to spherical coordination
  Z_[i] = z_[i];

  std::size_t const offset_i = neighbors_offset_[i];
  // Setup neighbours and neighbours-of-neighbours of atom i

  double const flow = params_.flow;
  double const fhigh = params_.fhigh;

  auto r_i = &r_[offset_i];

  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    finiteforce_[jj] = (r_i[jj] > flow) && (r_i[jj] < fhigh);
  }  // Loop over jj

  auto dz_i = &dz_[offset_i];
  auto N_i = &N_[offset_i * 3];

  for (std::size_t jj = 0, jj3 = 0; jj < number_of_neighbors_i; ++jj) {
    double const cst = dz_i[jj];
    Dzdx_[jj3] = N_i[jj3] * cst;
    ++jj3;
    Dzdx_[jj3] = N_i[jj3] * cst;
    ++jj3;
    Dzdx_[jj3] = N_i[jj3] * cst;
    ++jj3;
  }  // Loop over jj

  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];

  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    std::size_t const j = neighbors_to_particles_i[jj];
    std::size_t const number_of_neighbors_j = number_of_neighbors_[j];

    auto finiteforce2 = &finiteforce2_(jj, 0);
    for (std::size_t kk = 0; kk < number_of_neighbors_j; ++kk) {
      finiteforce2[kk] = false;
    }  // Loop over kk

    auto Dzdxx = &Dzdxx_(jj, 0);
    for (std::size_t kk = 0; kk < number_of_neighbors_j * 3; ++kk) {
      Dzdxx[kk] = 0.0;
    }  // Loop over kk
  }    // Loop over jj

  if (z_[i] <= 4.0) {
    DihedralContribution(i);
    RepulsionContribution(i);
  }
  LinearContribution(i);
}

// Calculate dihedral contribution to Z_i.
void EDIPC::DihedralContribution(std::size_t const i) {
  std::size_t const offset_i = neighbors_offset_[i];
  auto r_i = &r_[offset_i];
  auto N_i = &N_[offset_i * 3];

  auto dz_i = &dz_[offset_i];
  auto p_i = &p_[offset_i];
  auto dp_i = &dp_[offset_i];

  double const phigh = params_.phigh;
  double const fhigh = params_.fhigh;
  double const Zdih = params_.Zdih;

  // temporary variables
  VectorOfSizeDIM Rx[3];
  VectorOfSizeDIM rr;
  VectorOfSizeDIM pp;
  VectorOfSizeDIM dpp;
  VectorOfSizeDIM dgg;
  VectorOfSizeDIM Dzr[3];
  double dotp{};

  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];

  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    if (r_i[jj] >= phigh) {
      continue;
    }

    std::size_t const j = neighbors_to_particles_i[jj];
    if (z_[j] >= 4.0) {
      continue;
    }

    std::size_t const jj30 = jj * 3;
    std::size_t const jj31 = jj30 + 1;
    std::size_t const jj32 = jj30 + 2;

    std::size_t const number_of_neighbors_j = number_of_neighbors_[j];
    std::size_t const offset_j = neighbors_offset_[j];
    auto neighbors_to_particles_j = &neighbors_to_particles_[offset_j];
    auto r_j = &r_[offset_j];
    auto N_j = &N_[offset_j * 3];

    auto dz_j = &dz_[offset_j];
    auto p_j = &p_[offset_j];
    auto dp_j = &dp_[offset_j];

    auto finiteforce2 = &finiteforce2_(jj, 0);
    auto Dzdxx = &Dzdxx_(jj, 0);

    // Loop over all pairs of i's neighbours "kk" and "ll"
    for (std::size_t kk = 0; kk < number_of_neighbors_i; ++kk) {
      if (kk == jj || r_i[kk] >= phigh) {
        continue;
      }

      std::size_t const kk30 = kk * 3;
      std::size_t const kk31 = kk30 + 1;
      std::size_t const kk32 = kk30 + 2;

      for (std::size_t ll = kk + 1; ll < number_of_neighbors_i; ++ll) {
        if (ll == jj || r_i[ll] >= phigh) {
          continue;
        }

        std::size_t const ll30 = ll * 3;
        std::size_t const ll31 = ll30 + 1;
        std::size_t const ll32 = ll30 + 2;

        for (std::size_t mm = 0; mm < number_of_neighbors_j; ++mm) {
          if (r_j[mm] >= phigh) {
            continue;
          }

          std::size_t const m = neighbors_to_particles_j[mm];
          if (m == i) {
            continue;
          }

          std::size_t const mm30 = mm * 3;
          std::size_t const mm31 = mm30 + 1;
          std::size_t const mm32 = mm30 + 2;

          finiteforce_[jj] = true;
          finiteforce_[kk] = true;
          finiteforce_[ll] = true;
          finiteforce2[mm] = true;

          // Set up vectors: 0=ik, 1=il, 2=jm

          Rx[0][0] = N_i[kk30];
          Rx[0][1] = N_i[kk31];
          Rx[0][2] = N_i[kk32];

          Rx[1][0] = N_i[ll30];
          Rx[1][1] = N_i[ll31];
          Rx[1][2] = N_i[ll32];

          Rx[2][0] = N_j[mm30];
          Rx[2][1] = N_j[mm31];
          Rx[2][2] = N_j[mm32];

          rr[0] = r_i[kk];
          rr[1] = r_i[ll];
          rr[2] = r_j[mm];

          pp[0] = p_i[kk];
          pp[1] = p_i[ll];
          pp[2] = p_j[mm];

          double const pp4 = pp[0] * pp[1] * pp[2] * p_i[jj];

          dpp[0] = dp_i[kk];
          dpp[1] = dp_i[ll];
          dpp[2] = dp_j[mm];

          double const gg = pi3_[i] * pi3_[j];

          dgg[0] = dz_i[kk] * dpi3_[i] * pi3_[j];
          dgg[1] = dz_i[ll] * dpi3_[i] * pi3_[j];
          dgg[2] = dz_j[mm] * pi3_[i] * dpi3_[j];

          // The main calculation of the dihedral part of Z:
          // pi3(i)*pi3(j) * (Rjm . Rik x Ril)^2 * p(Rij) * p(Rik) * p(Ril) *
          // p(Rjm)

          // n1 = 0, n2 = 1, n3 = 2
          {
            VectorOfSizeDIM Xk;
            special::Cross3(Rx[1], Rx[2], Xk);
            dotp = special::Dot3(Rx[0], Xk);
            double const dz0 = pp[1] * pp[2] * p_i[jj];
            double const dz1 =
                dotp * dotp * dz0 * (dgg[0] * pp[0] + gg * dpp[0]);
            double const dz2 = gg * pp4 * 2.0 / rr[0] * dotp;
            double const cst = dz1 - dotp * dz2;
            Dzr[0][0] = Rx[0][0] * cst + Xk[0] * dz2;
            Dzr[0][1] = Rx[0][1] * cst + Xk[1] * dz2;
            Dzr[0][2] = Rx[0][2] * cst + Xk[2] * dz2;
          }
          // n1 = 1, n2 = 2, n3 = 0
          {
            VectorOfSizeDIM Xk;
            special::Cross3(Rx[2], Rx[0], Xk);
            dotp = special::Dot3(Rx[1], Xk);
            double const dz0 = pp[2] * pp[0] * p_i[jj];
            double const dz1 =
                dotp * dotp * dz0 * (dgg[1] * pp[1] + gg * dpp[1]);
            double const dz2 = gg * pp4 * 2.0 / rr[1] * dotp;
            double const cst = dz1 - dotp * dz2;
            Dzr[1][0] = Rx[1][0] * cst + Xk[0] * dz2;
            Dzr[1][1] = Rx[1][1] * cst + Xk[1] * dz2;
            Dzr[1][2] = Rx[1][2] * cst + Xk[2] * dz2;
          }
          // n1 = 2, n2 = 0, n3 = 1
          {
            VectorOfSizeDIM Xk;
            special::Cross3(Rx[0], Rx[1], Xk);
            dotp = special::Dot3(Rx[2], Xk);
            double const dz0 = pp[0] * pp[1] * p_i[jj];
            double const dz1 =
                dotp * dotp * dz0 * (dgg[2] * pp[2] + gg * dpp[2]);
            double const dz2 = gg * pp4 * 2.0 / rr[2] * dotp;
            double const cst = dz1 - dotp * dz2;
            Dzr[2][0] = Rx[2][0] * cst + Xk[0] * dz2;
            Dzr[2][1] = Rx[2][1] * cst + Xk[1] * dz2;
            Dzr[2][2] = Rx[2][2] * cst + Xk[2] * dz2;
          }

          Z_[i] += Zdih * (gg * dotp * dotp * pp4);

          Dzdx_[kk30] += Dzr[0][0] * Zdih;
          Dzdx_[kk31] += Dzr[0][1] * Zdih;
          Dzdx_[kk32] += Dzr[0][2] * Zdih;

          Dzdx_[ll30] += Dzr[1][0] * Zdih;
          Dzdx_[ll31] += Dzr[1][1] * Zdih;
          Dzdx_[ll32] += Dzr[1][2] * Zdih;

          Dzdxx[mm30] += Dzr[2][0] * Zdih;
          Dzdxx[mm31] += Dzr[2][1] * Zdih;
          Dzdxx[mm32] += Dzr[2][2] * Zdih;

          // Force between atoms i and j

          double const bit1 =
              (pi3_[i] * dpi3_[j] + dpi3_[i] * pi3_[j]) * p_i[jj] * dz_i[jj];
          double const bit2 = pi3_[i] * pi3_[j] * dp_i[jj];
          double const fac =
              Zdih * dotp * dotp * pp[0] * pp[1] * pp[2] * (bit1 + bit2);

          Dzdx_[jj30] += N_i[jj30] * fac;
          Dzdx_[jj31] += N_i[jj31] * fac;
          Dzdx_[jj32] += N_i[jj32] * fac;

          // If neighbour of i: dpi3(Zi) * pi3(Zj) * dot^2 * pp4
          for (std::size_t nni = 0; nni < number_of_neighbors_i; ++nni) {
            if (nni == jj || nni == kk || nni == ll || r_i[nni] >= fhigh) {
              continue;
            }

            finiteforce_[nni] = true;

            double const cst =
                Zdih * dpi3_[i] * dz_i[nni] * pi3_[j] * dotp * dotp * pp4;

            std::size_t nni3 = nni * 3;
            Dzdx_[nni3] += N_i[nni3] * cst;
            ++nni3;
            Dzdx_[nni3] += N_i[nni3] * cst;
            ++nni3;
            Dzdx_[nni3] += N_i[nni3] * cst;
          }  // end loop nni

          // if neighbour of j: dpi3(Zj) * pi3(Zi) * dot^2 * pp4
          for (std::size_t nnj = 0; nnj < number_of_neighbors_j; ++nnj) {
            if (nnj == mm || r_j[nnj] >= fhigh) {
              continue;
            }

            std::size_t const n = neighbors_to_particles_j[nnj];
            if (n == i) {
              continue;
            }

            finiteforce2[nnj] = true;

            double const cst =
                Zdih * dpi3_[j] * dz_j[nnj] * pi3_[i] * dotp * dotp * pp4;

            std::size_t nnj3 = nnj * 3;
            Dzdxx[nnj3] += N_j[nnj3] * cst;
            ++nnj3;
            Dzdxx[nnj3] += N_j[nnj3] * cst;
            ++nnj3;
            Dzdxx[nnj3] += N_j[nnj3] * cst;
          }  // end loop nnj
        }    // end loop mm
      }      // end loop ll
    }        // end loop kk
  }          // end loop jj
}

// Calculate repulsion contribution to Z_i.
void EDIPC::RepulsionContribution(std::size_t const i) {
  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  std::size_t const offset_i = neighbors_offset_[i];
  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];
  auto r_i = &r_[offset_i];
  auto N_i = &N_[offset_i * 3];

  auto dz_i = &dz_[offset_i];
  auto p_i = &p_[offset_i];
  auto dp_i = &dp_[offset_i];

  double const fhigh = params_.fhigh;
  double const plow = params_.plow;
  double const phigh = params_.phigh;
  double const c0 = params_.c0;
  double const Zrep = params_.Zrep;

  // temporary variables
  VectorOfSizeDIM Rx[3];
  VectorOfSizeDIM rr;
  VectorOfSizeDIM pp;
  VectorOfSizeDIM dpp;
  VectorOfSizeDIM dgg;
  VectorOfSizeDIM Dzr[3];
  double dotp{};

  // Loop over all neighbours "jj"
  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    if (r_i[jj] <= plow || r_i[jj] >= c0) {
      continue;
    }

    std::size_t const j = neighbors_to_particles_i[jj];
    if (z_[j] >= 4.0) {
      continue;
    }

    std::size_t const jj30 = jj * 3;
    std::size_t const jj31 = jj30 + 1;
    std::size_t const jj32 = jj30 + 2;

    std::size_t const number_of_neighbors_j = number_of_neighbors_[j];
    std::size_t const offset_j = neighbors_offset_[j];
    auto neighbors_to_particles_j = &neighbors_to_particles_[offset_j];
    auto r_j = &r_[offset_j];
    auto N_j = &N_[offset_j * 3];

    auto dz_j = &dz_[offset_j];

    auto finiteforce2 = &finiteforce2_(jj, 0);
    auto Dzdxx = &Dzdxx_(jj, 0);

    // Loop over all neighbours "kk"
    for (std::size_t kk = 0; kk < number_of_neighbors_i; ++kk) {
      if (kk == jj || r_i[kk] >= phigh) {
        continue;
      }

      std::size_t const kk30 = kk * 3;
      std::size_t const kk31 = kk30 + 1;
      std::size_t const kk32 = kk30 + 2;

      // Loop over all neighbours "kk"
      for (std::size_t ll = kk + 1; ll < number_of_neighbors_i; ++ll) {
        if (ll == jj || r_i[ll] >= phigh) {
          continue;
        }

        std::size_t const ll30 = ll * 3;
        std::size_t const ll31 = ll30 + 1;
        std::size_t const ll32 = ll30 + 2;

        finiteforce_[jj] = true;
        finiteforce_[kk] = true;
        finiteforce_[ll] = true;

        // Set up vectors: 0=ij, 1=ik

        Rx[0][0] = N_i[jj30];
        Rx[0][1] = N_i[jj31];
        Rx[0][2] = N_i[jj32];

        Rx[1][0] = N_i[kk30];
        Rx[1][1] = N_i[kk31];
        Rx[1][2] = N_i[kk32];

        Rx[2][0] = N_i[ll30];
        Rx[2][1] = N_i[ll31];
        Rx[2][2] = N_i[ll32];

        rr[0] = r_i[jj];
        rr[1] = r_i[kk];
        rr[2] = r_i[ll];

        double const cc = rr[0] - c0;

        pp[0] = (1.0 - p_i[jj]) * cc * cc;
        pp[1] = p_i[kk];
        pp[2] = p_i[ll];

        double const pp4 = pp[0] * pp[1] * pp[2];

        dpp[0] = -dp_i[jj] * cc * cc + (1.0 - p_i[jj]) * 2.0 * cc;
        dpp[1] = dp_i[kk];
        dpp[2] = dp_i[ll];

        double const gg = pi3_[i] * pi_[j];

        dgg[0] = dz_i[jj] * (dpi3_[i] * pi_[j] + dpi_[j] * pi3_[i]);
        dgg[1] = dz_i[kk] * dpi3_[i] * pi_[j];
        dgg[2] = dz_i[ll] * dpi3_[i] * pi_[j];

        // The main calculation of the repulsion part of Z:
        // pi3(i)*pi(j) * (Rij . Rik x Ril)^2 * p(Rij) * p(Rik) * (1-p(Ril)) *
        // (Ril-c0)^2

        // n1 = 0, n2 = 1, n3 = 2
        {
          VectorOfSizeDIM Xk;
          special::Cross3(Rx[1], Rx[2], Xk);
          dotp = special::Dot3(Rx[0], Xk);
          double const dz1 =
              dotp * dotp * pp[1] * pp[2] * (dgg[0] * pp[0] + gg * dpp[0]);
          double const dz2 = gg * pp4 * 2.0 / rr[0] * dotp;
          double const cst = dz1 - dotp * dz2;
          Dzr[0][0] = Rx[0][0] * cst + Xk[0] * dz2;
          Dzr[0][1] = Rx[0][1] * cst + Xk[1] * dz2;
          Dzr[0][2] = Rx[0][2] * cst + Xk[2] * dz2;
        }
        // n1 = 1, n2 = 2, n3 = 0
        {
          VectorOfSizeDIM Xk;
          special::Cross3(Rx[2], Rx[0], Xk);
          dotp = special::Dot3(Rx[1], Xk);
          double const dz1 =
              dotp * dotp * pp[2] * pp[0] * (dgg[1] * pp[1] + gg * dpp[1]);
          double const dz2 = gg * pp4 * 2.0 / rr[1] * dotp;
          double const cst = dz1 - dotp * dz2;
          Dzr[1][0] = Rx[1][0] * cst + Xk[0] * dz2;
          Dzr[1][1] = Rx[1][1] * cst + Xk[1] * dz2;
          Dzr[1][2] = Rx[1][2] * cst + Xk[2] * dz2;
        }
        // n1 = 2, n2 = 0, n3 = 1
        {
          VectorOfSizeDIM Xk;
          special::Cross3(Rx[0], Rx[1], Xk);
          dotp = special::Dot3(Rx[2], Xk);
          double const dz1 =
              dotp * dotp * pp[0] * pp[1] * (dgg[2] * pp[2] + gg * dpp[2]);
          double const dz2 = gg * pp4 * 2.0 / rr[2] * dotp;
          double const cst = dz1 - dotp * dz2;
          Dzr[2][0] = Rx[2][0] * cst + Xk[0] * dz2;
          Dzr[2][1] = Rx[2][1] * cst + Xk[1] * dz2;
          Dzr[2][2] = Rx[2][2] * cst + Xk[2] * dz2;
        }

        Z_[i] += Zrep * gg * dotp * dotp * pp4;

        Dzdx_[jj30] += Dzr[0][0] * Zrep;
        Dzdx_[jj31] += Dzr[0][1] * Zrep;
        Dzdx_[jj32] += Dzr[0][2] * Zrep;

        Dzdx_[kk30] += Dzr[1][0] * Zrep;
        Dzdx_[kk31] += Dzr[1][1] * Zrep;
        Dzdx_[kk32] += Dzr[1][2] * Zrep;

        Dzdx_[ll30] += Dzr[2][0] * Zrep;
        Dzdx_[ll31] += Dzr[2][1] * Zrep;
        Dzdx_[ll32] += Dzr[2][2] * Zrep;

        // Neighbours of i: easy derivative
        for (std::size_t nni = 0; nni < number_of_neighbors_i; ++nni) {
          if (nni == jj || nni == kk || nni == ll || r_i[nni] >= fhigh) {
            continue;
          }

          finiteforce_[nni] = true;

          double const cst =
              Zrep * dpi3_[i] * dz_i[nni] * pi_[j] * dotp * dotp * pp4;

          std::size_t nni3 = nni * 3;
          Dzdx_[nni3] += N_i[nni3] * cst;
          ++nni3;
          Dzdx_[nni3] += N_i[nni3] * cst;
          ++nni3;
          Dzdx_[nni3] += N_i[nni3] * cst;
        }  // end loop nni

        // Neighbours of j: easy derivative
        for (std::size_t nnj = 0; nnj < number_of_neighbors_j; ++nnj) {
          if (r_j[nnj] >= fhigh) {
            continue;
          }

          std::size_t const n = neighbors_to_particles_j[nnj];
          if (n == i) {
            continue;
          }

          finiteforce2[nnj] = true;

          double const cst =
              Zrep * dpi_[j] * dz_j[nnj] * pi3_[i] * dotp * dotp * pp4;

          std::size_t nnj3 = nnj * 3;
          Dzdxx[nnj3] += N_j[nnj3] * cst;
          ++nnj3;
          Dzdxx[nnj3] += N_j[nnj3] * cst;
          ++nnj3;
          Dzdxx[nnj3] += N_j[nnj3] * cst;
        }  // end loop nnj
      }    // end loop ll
    }      // end loop kk
  }        // end loop jj
}

// Calculate linear contribution to Z_i.
void EDIPC::LinearContribution(std::size_t const i) {
  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  std::size_t const offset_i = neighbors_offset_[i];
  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];
  auto r_i = &r_[offset_i];
  auto N_i = &N_[offset_i * 3];

  auto dz_i = &dz_[offset_i];
  auto p_i = &p_[offset_i];
  auto dp_i = &dp_[offset_i];

  double const fhigh = params_.fhigh;
  double const plow = params_.plow;
  double const phigh = params_.phigh;
  double const c0 = params_.c0;
  double const Zrep = params_.Zrep;

  // temporary variables
  VectorOfSizeDIM Rx[2];
  VectorOfSizeDIM rr;
  VectorOfSizeDIM pp;
  VectorOfSizeDIM dpp;
  VectorOfSizeDIM dgg;
  VectorOfSizeDIM Dzr[2];
  double dotp;

  // Loop over all neighbours "jj"
  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    if (r_i[jj] <= plow || r_i[jj] >= c0) {
      continue;
    }

    std::size_t const j = neighbors_to_particles_i[jj];
    if (z_[j] >= 4.0) {
      continue;
    }

    std::size_t const jj30 = jj * 3;
    std::size_t const jj31 = jj30 + 1;
    std::size_t const jj32 = jj30 + 2;

    std::size_t const number_of_neighbors_j = number_of_neighbors_[j];
    std::size_t const offset_j = neighbors_offset_[j];
    auto neighbors_to_particles_j = &neighbors_to_particles_[offset_j];
    auto r_j = &r_[offset_j];
    auto N_j = &N_[offset_j * 3];

    auto dz_j = &dz_[offset_j];

    auto finiteforce2 = &finiteforce2_(jj, 0);
    auto Dzdxx = &Dzdxx_(jj, 0);

    // Loop over all neighbours "kk"
    for (std::size_t kk = 0; kk < number_of_neighbors_i; ++kk) {
      if (kk == jj || r_i[kk] >= phigh) {
        continue;
      }

      std::size_t const kk30 = kk * 3;
      std::size_t const kk31 = kk30 + 1;
      std::size_t const kk32 = kk30 + 2;

      finiteforce_[jj] = true;
      finiteforce_[kk] = true;

      // Set up vectors: 0=ij, 1=ik

      Rx[0][0] = N_i[jj30];
      Rx[0][1] = N_i[jj31];
      Rx[0][2] = N_i[jj32];

      Rx[1][0] = N_i[kk30];
      Rx[1][1] = N_i[kk31];
      Rx[1][2] = N_i[kk32];

      rr[0] = r_i[jj];
      rr[1] = r_i[kk];

      double const cc = rr[0] - c0;

      pp[0] = (1.0 - p_i[jj]) * cc * cc;
      pp[1] = p_i[kk];

      double const pp2 = pp[0] * pp[1];

      dpp[0] = -dp_i[jj] * cc * cc + (1.0 - p_i[jj]) * 2.0 * cc;
      dpp[1] = dp_i[kk];

      double const gg = pi2_[i] * pi_[j];

      dgg[0] = dz_i[jj] * (dpi2_[i] * pi_[j] + pi2_[i] * dpi_[j]);
      dgg[1] = dz_i[kk] * dpi2_[i] * pi_[j];

      dotp = special::Dot3(N_i + jj30, N_i + kk30);

      // The main calculation of the linear part of Z:
      // pi2(i)*pi(j) * [1 - (Rij.Rik)^2] * p(Rij) * (1 - p(Rik)) * (Rij -
      // c0)^2

      // n1 = 0, n2 = 1
      {
        double const cst1 =
            ((1.0 - dotp * dotp) * pp[1] * (dgg[0] * pp[0] + gg * dpp[0]));
        double const cst2 = (pp2 * gg * 2.0 * dotp / rr[0]);
        Dzr[0][0] = Rx[0][0] * cst1 - (Rx[1][0] - Rx[0][0] * dotp) * cst2;
        Dzr[0][1] = Rx[0][1] * cst1 - (Rx[1][1] - Rx[0][1] * dotp) * cst2;
        Dzr[0][2] = Rx[0][2] * cst1 - (Rx[1][2] - Rx[0][2] * dotp) * cst2;
      }
      // n1 = 1, n2 = 0
      {
        double const cst1 =
            ((1.0 - dotp * dotp) * pp[0] * (dgg[1] * pp[1] + gg * dpp[1]));
        double const cst2 = (pp2 * gg * 2.0 * dotp / rr[1]);
        Dzr[1][0] = Rx[1][0] * cst1 - (Rx[0][0] - Rx[1][0] * dotp) * cst2;
        Dzr[1][1] = Rx[1][1] * cst1 - (Rx[0][1] - Rx[1][1] * dotp) * cst2;
        Dzr[1][2] = Rx[1][2] * cst1 - (Rx[0][2] - Rx[1][2] * dotp) * cst2;
      }

      Z_[i] += Zrep * gg * (1 - dotp * dotp) * pp2;

      Dzdx_[jj30] += Dzr[0][0] * Zrep;
      Dzdx_[jj31] += Dzr[0][1] * Zrep;
      Dzdx_[jj32] += Dzr[0][2] * Zrep;

      Dzdx_[kk30] += Dzr[1][0] * Zrep;
      Dzdx_[kk31] += Dzr[1][1] * Zrep;
      Dzdx_[kk32] += Dzr[1][2] * Zrep;

      // neighbours of i: easy derivative
      for (std::size_t nni = 0; nni < number_of_neighbors_i; ++nni) {
        if (nni == jj || nni == kk || r_i[nni] >= fhigh) {
          continue;
        }

        finiteforce_[nni] = true;

        double const cst =
            Zrep * dpi2_[i] * dz_i[nni] * pi_[j] * (1.0 - dotp * dotp) * pp2;

        std::size_t nni3 = nni * 3;
        Dzdx_[nni3] += N_i[nni3] * cst;
        ++nni3;
        Dzdx_[nni3] += N_i[nni3] * cst;
        ++nni3;
        Dzdx_[nni3] += N_i[nni3] * cst;
      }  // end loop nni

      // neighbours of j: easy derivative
      for (std::size_t nnj = 0; nnj < number_of_neighbors_j; ++nnj) {
        if (r_j[nnj] >= fhigh) {
          continue;
        }

        std::size_t const n = neighbors_to_particles_j[nnj];
        if (n == i) {
          continue;
        }

        finiteforce2[nnj] = true;

        double const cst =
            Zrep * dpi_[j] * dz_j[nnj] * pi2_[i] * (1.0 - dotp * dotp) * pp2;

        std::size_t nnj3 = nnj * 3;
        Dzdxx[nnj3] += N_j[nnj3] * cst;
        ++nnj3;
        Dzdxx[nnj3] += N_j[nnj3] * cst;
        ++nnj3;
        Dzdxx[nnj3] += N_j[nnj3] * cst;
      }  // end loop nnj
    }    // end loop kk
  }      // end loop jj
}

double EDIPC::CalcU2(std::size_t const i, double const r_i_jj, double &u2_arg,
                     double &u2_bond, double &u2_r5, double &u2_part1,
                     double &u2_part2, double &f22) const {
  double const Z_i = Z_[i];
  double const arg = r_i_jj - params_.a - params_.aprime * Z_i;
  u2_arg = 1.0 / arg;
  double const bond = -params_.beta * Z_i * Z_i;
  u2_bond = std::exp(bond);
  double const r_i_jj2 = r_i_jj * r_i_jj;
  double const r2 = 1.0 / r_i_jj2;
  double const r4 = r2 * r2;
  u2_r5 = r4 / r_i_jj;
  u2_part1 = params_.BB * r4 - u2_bond;
  double const part2 = params_.sigma * u2_arg;
  u2_part2 = params_.eps * std::exp(part2);
  // This is used for later force calculation
  double const cst1 = 2.0 * params_.beta * Z_i * u2_bond;
  double const cst2 = u2_part1 * part2 * u2_arg * params_.aprime;
  f22 = u2_part2 * (cst1 + cst2);
  return u2_part1 * u2_part2;
}

void EDIPC::CalcF2(std::size_t const i, std::size_t const jj,
                   std::size_t const kk, double const u2_arg,
                   double const u2_bond, double const u2_r5,
                   double const u2_part1, double const u2_part2,
                   VectorOfSizeDIM f2, VectorOfSizeDIM dxr) const {
  std::size_t const kk30 = kk * 3;
  std::size_t const kk31 = kk30 + 1;
  std::size_t const kk32 = kk30 + 2;

  std::size_t const offset_i = neighbors_offset_[i];
  auto N_i = &N_[offset_i * 3];

  if (jj == kk) {
    dxr[0] = N_i[kk30];
    dxr[1] = N_i[kk31];
    dxr[2] = N_i[kk32];
  } else {
    dxr[0] = 0.0;
    dxr[1] = 0.0;
    dxr[2] = 0.0;
  }

  double const cst2 = -4.0 * params_.BB * u2_r5;
  double const cst3 = 2.0 * params_.beta * Z_[i] * u2_bond;

  VectorOfSizeDIM part3;
  part3[0] = dxr[0] * cst2 + Dzdx_[kk30] * cst3;
  part3[1] = dxr[1] * cst2 + Dzdx_[kk31] * cst3;
  part3[2] = dxr[2] * cst2 + Dzdx_[kk32] * cst3;

  double const cst4 = -params_.sigma * u2_arg * u2_arg;

  VectorOfSizeDIM part4;
  part4[0] = (dxr[0] - Dzdx_[kk30] * params_.aprime) * cst4;
  part4[1] = (dxr[1] - Dzdx_[kk31] * params_.aprime) * cst4;
  part4[2] = (dxr[2] - Dzdx_[kk32] * params_.aprime) * cst4;

  f2[0] = (part3[0] + part4[0] * u2_part1) * u2_part2;
  f2[1] = (part3[1] + part4[1] * u2_part1) * u2_part2;
  f2[2] = (part3[2] + part4[2] * u2_part1) * u2_part2;
}

// Calculate tau and dtau from Z[i]
double EDIPC::TauFunction(std::size_t const i, double &dtau) const {
  double const Z = Z_[i];
  double const t2Z = edip_c::t2 * Z;
  double const tt = edip_c::t1 + t2Z;
  double const ttt = std::tanh(tt);
  double const fac = edip_c::kOneTwelve * (1.0 + ttt);
  double const ch = std::cosh(tt);
  double const ch2 = 12.0 * ch * ch;
  dtau = -t2Z / ch2 - fac;
  double const tau = 1.0 - Z * fac;
  return tau;
}

double EDIPC::CalcU3(std::size_t const i, std::size_t const jj,
                     std::size_t const kk, double const r_i_jj,
                     double const r_i_kk, double const tau, double const dtau,
                     VectorOfSizeDIM dcosj, VectorOfSizeDIM dcosk,
                     double &zexpgam, double &dtheta, double &u3_arg11,
                     double &u3_arg22, double &u3_argz, double &f33) const {
  std::size_t const offset_i = neighbors_offset_[i];
  auto N_i = &N_[offset_i * 3];

  std::size_t const jj30 = jj * 3;
  std::size_t const jj31 = jj30 + 1;
  std::size_t const jj32 = jj30 + 2;

  std::size_t const kk30 = kk * 3;
  std::size_t const kk31 = kk30 + 1;
  std::size_t const kk32 = kk30 + 2;

  double const r_i_jj_inverse = 1.0 / r_i_jj;
  double const r_i_kk_inverse = 1.0 / r_i_kk;

  // Calculate cos(theta_jik) and its derivatives
  double const cosi = special::Dot3(N_i + jj30, N_i + kk30);

  dcosj[0] = N_i[kk30] - N_i[jj30] * cosi;
  dcosj[1] = N_i[kk31] - N_i[jj31] * cosi;
  dcosj[2] = N_i[kk32] - N_i[jj32] * cosi;
  dcosj[0] *= r_i_jj_inverse;
  dcosj[1] *= r_i_jj_inverse;
  dcosj[2] *= r_i_jj_inverse;

  dcosk[0] = N_i[jj30] - N_i[kk30] * cosi;
  dcosk[1] = N_i[jj31] - N_i[kk31] * cosi;
  dcosk[2] = N_i[jj32] - N_i[kk32] * cosi;
  dcosk[0] *= r_i_kk_inverse;
  dcosk[1] *= r_i_kk_inverse;
  dcosk[2] *= r_i_kk_inverse;

  // Terms for u3 energy and constants for force loops
  double const Z = Z_[i];
  double const arg0 = params_.a + params_.aprime * Z;
  double const arg1 = 1.0 / (r_i_jj - arg0);
  double const arg2 = 1.0 / (r_i_kk - arg0);
  double const Zi0 = Z - params_.Z0;
  double const cst1 = params_.gamma * (arg1 + arg2);
  double const cst2 = params_.lambdaprime * Zi0 * Zi0;
  double const cst12 = std::exp(cst1 - cst2);
  zexpgam = params_.lambda0 * cst12;
  double const ct = cosi + tau;
  double const pt = params_.q * ct;
  double const cst3 = -pt * ct;
  double const expcos = std::exp(cst3);
  double const theta = 1.0 - expcos;
  dtheta = 2.0 * pt * expcos;
  double const u3term = zexpgam * theta;

  // These are used for later force calculations
  u3_arg11 = u3term * params_.gamma * arg1 * arg1;
  u3_arg22 = u3term * params_.gamma * arg2 * arg2;
  u3_argz = -u3term * 2.0 * params_.lambdaprime * Zi0;
  f33 = zexpgam * dtheta * dtau + params_.aprime * (u3_arg11 + u3_arg22) +
        u3_argz;

  return u3term;
}

void EDIPC::CalcF3(std::size_t const i, std::size_t const jj,
                   std::size_t const kk, std::size_t const ll,
                   double const dtau, VectorOfSizeDIM dcosj,
                   VectorOfSizeDIM dcosk, double const zexpgam,
                   double const dtheta, double const u3_arg11,
                   double const u3_arg22, double const u3_argz,
                   VectorOfSizeDIM f3) const {
  std::size_t const offset_i = neighbors_offset_[i];
  auto N_i = &N_[offset_i * 3];

  std::size_t const jj3 = jj * 3;
  std::size_t const kk3 = kk * 3;
  std::size_t const ll3 = ll * 3;

  auto Dzdx = &Dzdx_[ll3];

  VectorOfSizeDIM dri;
  dri[0] = Dzdx[0] * params_.aprime;
  dri[1] = Dzdx[1] * params_.aprime;
  dri[2] = Dzdx[2] * params_.aprime;

  VectorOfSizeDIM drj{dri[0], dri[1], dri[2]};

  if (jj == ll) {
    dri[0] -= N_i[jj3];
    dri[1] -= N_i[jj3 + 1];
    dri[2] -= N_i[jj3 + 2];
  }
  dri[0] *= u3_arg11;
  dri[1] *= u3_arg11;
  dri[2] *= u3_arg11;

  if (kk == ll) {
    drj[0] -= N_i[kk3];
    drj[1] -= N_i[kk3 + 1];
    drj[2] -= N_i[kk3 + 2];
  }
  drj[0] *= u3_arg22;
  drj[1] *= u3_arg22;
  drj[2] *= u3_arg22;

  VectorOfSizeDIM drz;
  drz[0] = Dzdx[0] * u3_argz;
  drz[1] = Dzdx[1] * u3_argz;
  drz[2] = Dzdx[2] * u3_argz;

  drz[0] += dri[0] + drj[0];
  drz[1] += dri[1] + drj[1];
  drz[2] += dri[2] + drj[2];

  VectorOfSizeDIM cosjk;
  if (jj == ll) {
    cosjk[0] = dcosj[0];
    cosjk[1] = dcosj[1];
    cosjk[2] = dcosj[2];
  } else {
    cosjk[0] = 0.0;
    cosjk[1] = 0.0;
    cosjk[2] = 0.0;
  }

  if (kk == ll) {
    cosjk[0] += dcosk[0];
    cosjk[1] += dcosk[1];
    cosjk[2] += dcosk[2];
  }

  double const zd{zexpgam * dtheta};

  f3[0] = drz[0] + (cosjk[0] + Dzdx[0] * dtau) * zd;
  f3[1] = drz[1] + (cosjk[1] + Dzdx[1] * dtau) * zd;
  f3[2] = drz[2] + (cosjk[2] + Dzdx[2] * dtau) * zd;
}

template <bool IsComputeEnergy, bool IsComputeForces,
          bool IsComputeParticleEnergy, bool IsComputeVirial,
          bool IsComputeParticleVirial>
void EDIPC::Pair(std::size_t const i, int const *const particle_contributing,
                 double *const energy, VectorOfSizeDIM *const forces,
                 double *const particle_energy, VectorOfSizeSix virial,
                 VectorOfSizeSix *const particle_virial) {
  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  if (number_of_neighbors_i == 0) {
    return;
  }

  // Temporary variables
  double u2_arg;
  double u2_bond;
  double u2_r5;
  double u2_part1;
  double u2_part2;
  double f22;
  VectorOfSizeDIM f2;
  VectorOfSizeDIM dxr;

  std::size_t const offset_i = neighbors_offset_[i];
  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];
  auto r_i = &r_[offset_i];
  auto N_i = &N_[offset_i * 3];

  double const rlimit = params_.a + params_.aprime * Z_[i] - 0.001;

  // Loop over neighbours j to compute pair energy
  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    double const r_i_jj = r_i[jj];

    if (r_i_jj >= rlimit) {
      continue;
    }

    std::size_t const j = neighbors_to_particles_i[jj];

    // Pair energy
    double const u2 =
        CalcU2(i, r_i_jj, u2_arg, u2_bond, u2_r5, u2_part1, u2_part2, f22);

    if (IsComputeEnergy) {
      *energy += u2;
    }

    if (IsComputeParticleEnergy) {
      if (particle_contributing[j]) {
        double const u2half = 0.5 * u2;
        particle_energy[i] += u2half;
        particle_energy[j] += u2half;
      } else {
        particle_energy[i] += u2;
      }
    }  // IsComputeParticleEnergy

    if (IsComputeForces || IsComputeVirial || IsComputeParticleVirial) {
      for (std::size_t kk = 0; kk < number_of_neighbors_i; ++kk) {
        if ((jj != kk) && !finiteforce_[kk]) {
          continue;
        }

        std::size_t const k = neighbors_to_particles_i[kk];

        // Force due to neighbours of i
        CalcF2(i, jj, kk, u2_arg, u2_bond, u2_r5, u2_part1, u2_part2, f2, dxr);

        if (IsComputeForces) {
          forces[i][0] -= f2[0];
          forces[i][1] -= f2[1];
          forces[i][2] -= f2[2];

          forces[k][0] += f2[0];
          forces[k][1] += f2[1];
          forces[k][2] += f2[2];
        }  // IsComputeForces

        if (IsComputeVirial || IsComputeParticleVirial) {
          double const fpair = special::Len3(f2);

          double const r_i_kk = r_i[kk];

          std::size_t const kk3 = kk * 3;
          double const delx = N_i[kk3] * r_i_kk;
          double const dely = N_i[kk3 + 1] * r_i_kk;
          double const delz = N_i[kk3 + 2] * r_i_kk;

          // Virial has 6 components and is stored as a 6-element
          // vector in the following order: xx, yy, zz, yz, xz, xy.
          VectorOfSizeSix v;
          v[0] = delx * delx * fpair;
          v[1] = dely * dely * fpair;
          v[2] = delz * delz * fpair;
          v[3] = dely * delz * fpair;
          v[4] = delx * delz * fpair;
          v[5] = delx * dely * fpair;

          if (IsComputeVirial) {
            virial[0] += v[0];
            virial[1] += v[1];
            virial[2] += v[2];
            virial[3] += v[3];
            virial[4] += v[4];
            virial[5] += v[5];
          }

          if (IsComputeParticleVirial) {
            v[0] *= 0.5;
            v[1] *= 0.5;
            v[2] *= 0.5;
            v[3] *= 0.5;
            v[4] *= 0.5;
            v[5] *= 0.5;

            particle_virial[i][0] += v[0];
            particle_virial[i][1] += v[1];
            particle_virial[i][2] += v[2];
            particle_virial[i][3] += v[3];
            particle_virial[i][4] += v[4];
            particle_virial[i][5] += v[5];

            particle_virial[k][0] += v[0];
            particle_virial[k][1] += v[1];
            particle_virial[k][2] += v[2];
            particle_virial[k][3] += v[3];
            particle_virial[k][4] += v[4];
            particle_virial[k][5] += v[5];
          }  // IsComputeParticleVirial
        }    // IsComputeVirial || IsComputeParticleVirial

        std::size_t const number_of_neighbors_k = number_of_neighbors_[k];
        std::size_t const offset_k = neighbors_offset_[k];
        auto neighbors_to_particles_k = &neighbors_to_particles_[offset_k];
        auto r_k = &r_[offset_k];
        auto N_k = &N_[offset_k * 3];

        auto finiteforce2 = &finiteforce2_(kk, 0);
        auto Dzdxx = &Dzdxx_(kk, 0);

        for (std::size_t ll = 0; ll < number_of_neighbors_k; ++ll) {
          if (!finiteforce2[ll]) {
            continue;
          }

          std::size_t const l = neighbors_to_particles_k[ll];
          std::size_t const ll30 = ll * 3;
          std::size_t const ll31 = ll30 + 1;
          std::size_t const ll32 = ll30 + 2;

          // Force due to neighbours-of-neighbours of i
          f2[0] = Dzdxx[ll30] * f22;
          f2[1] = Dzdxx[ll31] * f22;
          f2[2] = Dzdxx[ll32] * f22;

          if (IsComputeForces) {
            forces[k][0] -= f2[0];
            forces[k][1] -= f2[1];
            forces[k][2] -= f2[2];

            forces[l][0] += f2[0];
            forces[l][1] += f2[1];
            forces[l][2] += f2[2];
          }  // IsComputeForces

          if (IsComputeVirial || IsComputeParticleVirial) {
            double const fpair = special::Len3(f2);
            double const r_k_ll = r_k[ll];

            double const delx = N_k[ll30] * r_k_ll;
            double const dely = N_k[ll31] * r_k_ll;
            double const delz = N_k[ll32] * r_k_ll;

            // Virial has 6 components and is stored as a 6-element
            // vector in the following order: xx, yy, zz, yz, xz, xy.
            VectorOfSizeSix v;
            v[0] = delx * delx * fpair;
            v[1] = dely * dely * fpair;
            v[2] = delz * delz * fpair;
            v[3] = dely * delz * fpair;
            v[4] = delx * delz * fpair;
            v[5] = delx * dely * fpair;

            if (IsComputeVirial) {
              virial[0] += v[0];
              virial[1] += v[1];
              virial[2] += v[2];
              virial[3] += v[3];
              virial[4] += v[4];
              virial[5] += v[5];
            }

            if (IsComputeParticleVirial) {
              v[0] *= 0.5;
              v[1] *= 0.5;
              v[2] *= 0.5;
              v[3] *= 0.5;
              v[4] *= 0.5;
              v[5] *= 0.5;

              particle_virial[k][0] += v[0];
              particle_virial[k][1] += v[1];
              particle_virial[k][2] += v[2];
              particle_virial[k][3] += v[3];
              particle_virial[k][4] += v[4];
              particle_virial[k][5] += v[5];

              particle_virial[l][0] += v[0];
              particle_virial[l][1] += v[1];
              particle_virial[l][2] += v[2];
              particle_virial[l][3] += v[3];
              particle_virial[l][4] += v[4];
              particle_virial[l][5] += v[5];
            }  // IsComputeParticleVirial
          }    // IsComputeVirial || IsComputeParticleVirial
        }      // end loop ll
      }        // end loop kk
    }          // IsComputeForces || IsComputeVirial || IsComputeParticleVirial
  }            // end loop jj
}

template <bool IsComputeEnergy, bool IsComputeForces,
          bool IsComputeParticleEnergy, bool IsComputeVirial,
          bool IsComputeParticleVirial>
void EDIPC::PairZbl(std::size_t const i, int const *const particle_contributing,
                    double *const energy, VectorOfSizeDIM *const forces,
                    double *const particle_energy, VectorOfSizeSix virial,
                    VectorOfSizeSix *const particle_virial) {
  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  if (number_of_neighbors_i == 0) {
    return;
  }

  // Temporary variables
  double u2_arg;
  double u2_bond;
  double u2_r5;
  double u2_part1;
  double u2_part2;
  double f22;
  VectorOfSizeDIM f2;
  VectorOfSizeDIM dxr;

  std::size_t const offset_i = neighbors_offset_[i];
  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];
  auto r_i = &r_[offset_i];
  auto N_i = &N_[offset_i * 3];

  double const rlimit = params_.a + params_.aprime * Z_[i] - 0.001;

  // Loop over neighbours j to compute pair energy
  for (std::size_t jj = 0; jj < number_of_neighbors_i; ++jj) {
    double const r_i_jj = r_i[jj];

    if (r_i_jj >= rlimit) {
      continue;
    }

    std::size_t const j = neighbors_to_particles_i[jj];
    double const r_i_jj_inverse = 1.0 / r_i_jj;

    // The ZBL pair term
    double dphi_zbl;
    double const phi_zbl =
        0.5 * zbl_->ComputePhi(r_i_jj, r_i_jj_inverse, dphi_zbl);
    dphi_zbl *= 0.5;

    // The CEDIP pair term
    double const uedip =
        CalcU2(i, r_i_jj, u2_arg, u2_bond, u2_r5, u2_part1, u2_part2, f22);

    // The Fermi switching functions
    double dzbl_sf;
    double const zbl_sf = fermi_->FermiSwitchingFunction(r_i_jj, dzbl_sf);

    double dedip_sf;
    double const edip_sf = fermi_->CFermiSwitchingFunction(r_i_jj, dedip_sf);

    if (IsComputeEnergy || IsComputeParticleEnergy) {
      double const u2 = uedip * edip_sf + phi_zbl * zbl_sf;

      if (IsComputeEnergy) {
        *energy += u2;
      }

      if (IsComputeParticleEnergy) {
        if (particle_contributing[j]) {
          double const u2half = 0.5 * u2;
          particle_energy[i] += u2half;
          particle_energy[j] += u2half;
        } else {
          particle_energy[i] += u2;
        }
      }  // IsComputeParticleEnergy
    }    // IsComputeEnergy || IsComputeParticleEnergy

    if (IsComputeForces || IsComputeVirial || IsComputeParticleVirial) {
      // Calculate the ZBL force using the chain rule
      double const fzbl = dphi_zbl * zbl_sf + dzbl_sf * phi_zbl;

      // Force due to neighbours of i
      CalcF2(i, jj, jj, u2_arg, u2_bond, u2_r5, u2_part1, u2_part2, f2, dxr);

      f2[0] *= edip_sf;
      f2[1] *= edip_sf;
      f2[2] *= edip_sf;

      double const cst = uedip * dedip_sf + fzbl;

      f2[0] += dxr[0] * cst;
      f2[1] += dxr[1] * cst;
      f2[2] += dxr[2] * cst;

      if (IsComputeForces) {
        forces[i][0] -= f2[0];
        forces[i][1] -= f2[1];
        forces[i][2] -= f2[2];

        forces[j][0] += f2[0];
        forces[j][1] += f2[1];
        forces[j][2] += f2[2];
      }  // IsComputeForces

      if (IsComputeVirial || IsComputeParticleVirial) {
        double const fpair = special::Len3(f2);

        std::size_t const jj3 = jj * 3;

        double const delx = N_i[jj3] * r_i_jj;
        double const dely = N_i[jj3 + 1] * r_i_jj;
        double const delz = N_i[jj3 + 2] * r_i_jj;

        // Virial has 6 components and is stored as a 6-element
        // vector in the following order: xx, yy, zz, yz, xz, xy.
        VectorOfSizeSix v;
        v[0] = delx * delx * fpair;
        v[1] = dely * dely * fpair;
        v[2] = delz * delz * fpair;
        v[3] = dely * delz * fpair;
        v[4] = delx * delz * fpair;
        v[5] = delx * dely * fpair;

        if (IsComputeVirial) {
          virial[0] += v[0];
          virial[1] += v[1];
          virial[2] += v[2];
          virial[3] += v[3];
          virial[4] += v[4];
          virial[5] += v[5];
        }

        if (IsComputeParticleVirial) {
          v[0] *= 0.5;
          v[1] *= 0.5;
          v[2] *= 0.5;
          v[3] *= 0.5;
          v[4] *= 0.5;
          v[5] *= 0.5;

          particle_virial[i][0] += v[0];
          particle_virial[i][1] += v[1];
          particle_virial[i][2] += v[2];
          particle_virial[i][3] += v[3];
          particle_virial[i][4] += v[4];
          particle_virial[i][5] += v[5];

          particle_virial[j][0] += v[0];
          particle_virial[j][1] += v[1];
          particle_virial[j][2] += v[2];
          particle_virial[j][3] += v[3];
          particle_virial[j][4] += v[4];
          particle_virial[j][5] += v[5];
        }  // IsComputeParticleVirial
      }    // IsComputeVirial || IsComputeParticleVirial

      double const f22_edip_sf = f22 * edip_sf;

      // Loop over neighbours k
      for (std::size_t kk = 0; kk < number_of_neighbors_i; ++kk) {
        if (!finiteforce_[kk]) {
          continue;
        }

        std::size_t const k = neighbors_to_particles_i[kk];

        // Force due to neighbours of k != j
        if (k != j) {
          std::size_t const kk30 = kk * 3;
          std::size_t const kk31 = kk30 + 1;
          std::size_t const kk32 = kk30 + 2;

          f2[0] = Dzdx_[kk30] * f22_edip_sf;
          f2[1] = Dzdx_[kk31] * f22_edip_sf;
          f2[2] = Dzdx_[kk32] * f22_edip_sf;

          if (IsComputeForces) {
            forces[i][0] -= f2[0];
            forces[i][1] -= f2[1];
            forces[i][2] -= f2[2];

            forces[k][0] += f2[0];
            forces[k][1] += f2[1];
            forces[k][2] += f2[2];
          }  // IsComputeForces

          if (IsComputeVirial || IsComputeParticleVirial) {
            double const fpair = special::Len3(f2);

            double const r_i_kk = r_i[kk];

            double const delx = N_i[kk30] * r_i_kk;
            double const dely = N_i[kk31] * r_i_kk;
            double const delz = N_i[kk32] * r_i_kk;

            // Virial has 6 components and is stored as a 6-element
            // vector in the following order: xx, yy, zz, yz, xz, xy.
            VectorOfSizeSix v;
            v[0] = delx * delx * fpair;
            v[1] = dely * dely * fpair;
            v[2] = delz * delz * fpair;
            v[3] = dely * delz * fpair;
            v[4] = delx * delz * fpair;
            v[5] = delx * dely * fpair;

            if (IsComputeVirial) {
              virial[0] += v[0];
              virial[1] += v[1];
              virial[2] += v[2];
              virial[3] += v[3];
              virial[4] += v[4];
              virial[5] += v[5];
            }

            if (IsComputeParticleVirial) {
              v[0] *= 0.5;
              v[1] *= 0.5;
              v[2] *= 0.5;
              v[3] *= 0.5;
              v[4] *= 0.5;
              v[5] *= 0.5;

              particle_virial[i][0] += v[0];
              particle_virial[i][1] += v[1];
              particle_virial[i][2] += v[2];
              particle_virial[i][3] += v[3];
              particle_virial[i][4] += v[4];
              particle_virial[i][5] += v[5];

              particle_virial[k][0] += v[0];
              particle_virial[k][1] += v[1];
              particle_virial[k][2] += v[2];
              particle_virial[k][3] += v[3];
              particle_virial[k][4] += v[4];
              particle_virial[k][5] += v[5];
            }  // IsComputeParticleVirial
          }    // IsComputeVirial || IsComputeParticleVirial

        }  // k != j

        std::size_t const number_of_neighbors_k = number_of_neighbors_[k];
        std::size_t const offset_k = neighbors_offset_[k];
        auto neighbors_to_particles_k = &neighbors_to_particles_[offset_k];
        auto r_k = &r_[offset_k];
        auto N_k = &N_[offset_k * 3];

        auto finiteforce2 = &finiteforce2_(kk, 0);
        auto Dzdxx = &Dzdxx_(kk, 0);

        for (std::size_t ll = 0; ll < number_of_neighbors_k; ++ll) {
          if (!finiteforce2[ll]) {
            continue;
          }

          std::size_t const l = neighbors_to_particles_k[ll];

          std::size_t const ll30 = ll * 3;
          std::size_t const ll31 = ll30 + 1;
          std::size_t const ll32 = ll30 + 2;

          // Force due to neighbours-of-neighbours of i
          f2[0] = Dzdxx[ll30] * f22_edip_sf;
          f2[1] = Dzdxx[ll31] * f22_edip_sf;
          f2[2] = Dzdxx[ll32] * f22_edip_sf;

          if (IsComputeForces) {
            forces[k][0] -= f2[0];
            forces[k][1] -= f2[1];
            forces[k][2] -= f2[2];

            forces[l][0] += f2[0];
            forces[l][1] += f2[1];
            forces[l][2] += f2[2];
          }  // IsComputeForces

          if (IsComputeVirial || IsComputeParticleVirial) {
            double const fpair = special::Len3(f2);

            double const r_k_ll = r_k[ll];

            double const delx = N_k[ll30] * r_k_ll;
            double const dely = N_k[ll31] * r_k_ll;
            double const delz = N_k[ll32] * r_k_ll;

            // Virial has 6 components and is stored as a 6-element
            // vector in the following order: xx, yy, zz, yz, xz, xy.
            VectorOfSizeSix v;
            v[0] = delx * delx * fpair;
            v[1] = dely * dely * fpair;
            v[2] = delz * delz * fpair;
            v[3] = dely * delz * fpair;
            v[4] = delx * delz * fpair;
            v[5] = delx * dely * fpair;

            if (IsComputeVirial) {
              virial[0] += v[0];
              virial[1] += v[1];
              virial[2] += v[2];
              virial[3] += v[3];
              virial[4] += v[4];
              virial[5] += v[5];
            }

            if (IsComputeParticleVirial) {
              v[0] *= 0.5;
              v[1] *= 0.5;
              v[2] *= 0.5;
              v[3] *= 0.5;
              v[4] *= 0.5;
              v[5] *= 0.5;

              particle_virial[k][0] += v[0];
              particle_virial[k][1] += v[1];
              particle_virial[k][2] += v[2];
              particle_virial[k][3] += v[3];
              particle_virial[k][4] += v[4];
              particle_virial[k][5] += v[5];

              particle_virial[l][0] += v[0];
              particle_virial[l][1] += v[1];
              particle_virial[l][2] += v[2];
              particle_virial[l][3] += v[3];
              particle_virial[l][4] += v[4];
              particle_virial[l][5] += v[5];
            }  // IsComputeParticleVirial
          }    // IsComputeVirial || IsComputeParticleVirial
        }      // Loop over ll
      }        // Loop over kk
    }          // IsComputeForces || IsComputeVirial || IsComputeParticleVirial
  }            // end loop jj
}

template <bool IsComputeEnergy, bool IsComputeForces,
          bool IsComputeParticleEnergy, bool IsComputeVirial,
          bool IsComputeParticleVirial>
void EDIPC::Triple(std::size_t const i, int const *const particle_contributing,
                   double *const energy, VectorOfSizeDIM *const forces,
                   double *const particle_energy, VectorOfSizeSix virial,
                   VectorOfSizeSix *const particle_virial) {
  std::size_t const number_of_neighbors_i = number_of_neighbors_[i];
  if (number_of_neighbors_i == 0) {
    return;
  }

  // Temporary variables
  VectorOfSizeDIM dcosj;
  VectorOfSizeDIM dcosk;
  double zexpgam;
  double dtheta;
  double u3_arg11;
  double u3_arg22;
  double u3_argz;
  double f33;
  VectorOfSizeDIM f3;

  // Calculate tau and dtau
  double dtau;
  double const tau = TauFunction(i, dtau);

  std::size_t const offset_i = neighbors_offset_[i];
  auto neighbors_to_particles_i = &neighbors_to_particles_[offset_i];
  auto r_i = &r_[offset_i];
  auto N_i = &N_[offset_i * 3];

  double const rlimit = params_.a + params_.aprime * Z_[i] - 0.001;

  // Loop over neighbours j to compute triple energy
  for (std::size_t jj = 0; jj < number_of_neighbors_i - 1; ++jj) {
    double const r_i_jj = r_i[jj];

    if (r_i_jj >= rlimit) {
      continue;
    }

    std::size_t const j = neighbors_to_particles_i[jj];

    for (std::size_t kk = jj + 1; kk < number_of_neighbors_i; ++kk) {
      double const r_i_kk = r_i[kk];

      if (r_i_kk >= rlimit) {
        continue;
      }

      double const u3 =
          CalcU3(i, jj, kk, r_i_jj, r_i_kk, tau, dtau, dcosj, dcosk, zexpgam,
                 dtheta, u3_arg11, u3_arg22, u3_argz, f33);

      if (IsComputeEnergy) {
        *energy += u3;
      }

      if (IsComputeParticleEnergy) {
        if (particle_contributing[j]) {
          double const u3half = 0.5 * u3;
          particle_energy[i] += u3half;
          particle_energy[j] += u3half;
        } else {
          particle_energy[i] += u3;
        }
      }  // IsComputeParticleEnergy

      if (IsComputeForces || IsComputeVirial || IsComputeParticleVirial) {
        // Forces due to neighbours of i
        for (std::size_t ll = 0; ll < number_of_neighbors_i; ++ll) {
          if ((jj != ll) && (kk != ll) && !finiteforce_[ll]) {
            continue;
          }

          std::size_t const l = neighbors_to_particles_i[ll];

          CalcF3(i, jj, kk, ll, dtau, dcosj, dcosk, zexpgam, dtheta, u3_arg11,
                 u3_arg22, u3_argz, f3);

          if (IsComputeForces) {
            forces[i][0] -= f3[0];
            forces[i][1] -= f3[1];
            forces[i][2] -= f3[2];

            forces[l][0] += f3[0];
            forces[l][1] += f3[1];
            forces[l][2] += f3[2];
          }  // IsComputeForces

          if (IsComputeVirial || IsComputeParticleVirial) {
            double const fpair = special::Len3(f3);

            double const r_i_ll = r_i[ll];

            std::size_t const ll3 = ll * 3;
            double const delx = N_i[ll3] * r_i_ll;
            double const dely = N_i[ll3 + 1] * r_i_ll;
            double const delz = N_i[ll3 + 2] * r_i_ll;

            // Virial has 6 components and is stored as a 6-element
            // vector in the following order: xx, yy, zz, yz, xz, xy.
            VectorOfSizeSix v;
            v[0] = delx * delx * fpair;
            v[1] = dely * dely * fpair;
            v[2] = delz * delz * fpair;
            v[3] = dely * delz * fpair;
            v[4] = delx * delz * fpair;
            v[5] = delx * dely * fpair;

            if (IsComputeVirial) {
              virial[0] += v[0];
              virial[1] += v[1];
              virial[2] += v[2];
              virial[3] += v[3];
              virial[4] += v[4];
              virial[5] += v[5];
            }

            if (IsComputeParticleVirial) {
              v[0] *= 0.5;
              v[1] *= 0.5;
              v[2] *= 0.5;
              v[3] *= 0.5;
              v[4] *= 0.5;
              v[5] *= 0.5;

              particle_virial[i][0] += v[0];
              particle_virial[i][1] += v[1];
              particle_virial[i][2] += v[2];
              particle_virial[i][3] += v[3];
              particle_virial[i][4] += v[4];
              particle_virial[i][5] += v[5];

              particle_virial[l][0] += v[0];
              particle_virial[l][1] += v[1];
              particle_virial[l][2] += v[2];
              particle_virial[l][3] += v[3];
              particle_virial[l][4] += v[4];
              particle_virial[l][5] += v[5];
            }  // IsComputeParticleVirial
          }    // IsComputeVirial || IsComputeParticleVirial

          std::size_t const offset_l = neighbors_offset_[l];
          auto neighbors_to_particles_l = &neighbors_to_particles_[offset_l];
          auto r_l = &r_[offset_l];
          auto N_l = &N_[offset_l * 3];

          auto finiteforce2 = &finiteforce2_(ll, 0);
          auto Dzdxx = &Dzdxx_(ll, 0);

          // Forces due to neighbours-of-neighbours of i
          for (std::size_t mm = 0; mm < number_of_neighbors_i; ++mm) {
            if (!finiteforce2[mm]) {
              continue;
            }

            std::size_t const m = neighbors_to_particles_l[mm];
            std::size_t const mm30 = mm * 3;
            std::size_t const mm31 = mm30 + 1;
            std::size_t const mm32 = mm30 + 2;

            f3[0] = Dzdxx[mm30] * f33;
            f3[1] = Dzdxx[mm31] * f33;
            f3[2] = Dzdxx[mm32] * f33;

            if (IsComputeForces) {
              forces[l][0] -= f3[0];
              forces[l][1] -= f3[1];
              forces[l][2] -= f3[2];

              forces[m][0] += f3[0];
              forces[m][1] += f3[1];
              forces[m][2] += f3[2];
            }  // IsComputeForces

            if (IsComputeVirial || IsComputeParticleVirial) {
              double const fpair = special::Len3(f3);

              double const r_l_mm = r_l[mm];

              double const delx = N_l[mm30] * r_l_mm;
              double const dely = N_l[mm31] * r_l_mm;
              double const delz = N_l[mm32] * r_l_mm;

              // Virial has 6 components and is stored as a 6-element
              // vector in the following order: xx, yy, zz, yz, xz, xy.
              VectorOfSizeSix v;
              v[0] = delx * delx * fpair;
              v[1] = dely * dely * fpair;
              v[2] = delz * delz * fpair;
              v[3] = dely * delz * fpair;
              v[4] = delx * delz * fpair;
              v[5] = delx * dely * fpair;

              if (IsComputeVirial) {
                virial[0] += v[0];
                virial[1] += v[1];
                virial[2] += v[2];
                virial[3] += v[3];
                virial[4] += v[4];
                virial[5] += v[5];
              }

              if (IsComputeParticleVirial) {
                v[0] *= 0.5;
                v[1] *= 0.5;
                v[2] *= 0.5;
                v[3] *= 0.5;
                v[4] *= 0.5;
                v[5] *= 0.5;

                particle_virial[l][0] += v[0];
                particle_virial[l][1] += v[1];
                particle_virial[l][2] += v[2];
                particle_virial[l][3] += v[3];
                particle_virial[l][4] += v[4];
                particle_virial[l][5] += v[5];

                particle_virial[m][0] += v[0];
                particle_virial[m][1] += v[1];
                particle_virial[m][2] += v[2];
                particle_virial[m][3] += v[3];
                particle_virial[m][4] += v[4];
                particle_virial[m][5] += v[5];
              }  // IsComputeParticleVirial
            }    // IsComputeVirial || IsComputeParticleVirial
          }      // Loop over mm
        }        // Loop over ll
      }  // IsComputeForces || IsComputeVirial || IsComputeParticleVirial
    }    // Loop over kk
  }      // Loop over jj
}

#include "edip_c_dispatch.cpp"